Robot control is a demanding issue for the success of a manipulation task of robots. This paper introduces a Matlab language-written software to simulate constrained dynamics, kinematics and control motion tasks of 6 axis articulated robots in some cases. This software is integrated in a Robot system with the Galil based-PC motion control for education and research, developing on mechatronics. Some algorithms related to Kinematics and Constrained Dynamics of the Robot with the environment are also mentioned. For illustration, some results of implementation are listed and future steps are also discussed.