moving real robot with setActiveDOFvalues()

[lakshmip001]

I am trying to execute the following function to move the robot arm but it doesn't move:

    def move(self):
        probot = self.env.GetRobots()[0]
        manip = probot.SetActiveManipulator(self.robot.GetManipulators()[0])
        print probot
        print manip
        x = manip.GetArmIndices()
        print x
        probot.SetActiveDOFs(x)
        numpy.random.seed(123)
        qstart = [-1.06, -1.5, -1.81, -2.98, -1.13, -3.14]
        probot.SetActiveDOFValues(qstart)
        Tstart = manip.GetEndEffectorTransform()
        twist = numpy.zeros(6)
        twist[:3] = [0.0, 0.2, 0.0]
        twist[3:] = numpy.deg2rad([10,10,10])
        velocity = 0.1
        traj = ru.planning.plan_constant_velocity_twist(self.robot, twist, velocity)
        print traj
        ros_traj = ru.planning.ros_trajectory_from_openrave(self.robot.GetName(), traj)
        print ros_traj
        qgoal = ros_traj.points[-1].positions
        print qgoal
        probot.SetActiveDOFValues(qgoal)
        Tgoal = manip.GetEndEffectorTransform()
        pos_diff = Tgoal[:3, 3] - Tstart[:3, 3]
        print pos_diff
        self.assertTrue(numpy.linalg.norm(pos_diff - twist[:3]) < 2e-3)

please help me if you have an idea.

[lakshmip001]

when trying to sent traj directly to execute_trajectory_wait_for_controller(traj) gives following error

Traceback (most recent call last):
  File "/home/lakshmi/cat_OR_ws/src/ur5controller/scripts/control_ur5.py", line 193, in <module>
    demo.control_robot_with_the_keyboard()
  File "/home/lakshmi/cat_OR_ws/src/ur5controller/scripts/control_ur5.py", line 188, in control_robot_with_the_keyboard
    self.move_constant_velocity()
  File "/home/lakshmi/cat_OR_ws/src/ur5controller/scripts/control_ur5.py", line 136, in move_constant_velocity
    self.robot.execute_trajectory_and_wait_for_controller(ros_traj)
  File "/home/lakshmi/cat_OR_ws/src/ur5controller/pythonsrc/ur5_robot/ur5_robot.py", line 145, in execute_trajectory_and_wait_for_controller
    self.GetController().SetPath(trajectory)
Boost.Python.ArgumentError: Python argument types in
    Controller.SetPath(Controller, JointTrajectory)
did not match C++ signature:
    SetPath(openravepy::PyControllerBase {lvalue}, boost::shared_ptr<openravepy::PyTrajectoryBase>)

[rpapallas]

Did you changed the trajectory yourself?

[lakshmip001]

Yes. I just added that function .

[rpapallas]

It looks like the trajectory you are generating is not compatible with what OpenRAVE expects. You should find a way to integrate the trajectory you manually generate to a valid OpenRAVE trajectory.

[rpapallas]

This doesn't look to be related with this controller but rather how the trajectory is generated, which looks to be invalid.