MoveToHandPosition Error

classic Classic list List threaded Threaded
1 message Options
Reply | Threaded
Open this post in threaded view
|

MoveToHandPosition Error

parshad.dp
Hi Rosen,

I am trying to move my arm after grasping the object using the following code as per the example given on openrave site:

if movehanddown:
            try:
                print 'move hand up'
                self.basemanip.MoveHandStraight(direction =[0,0,1],stepsize = 0.003,minsteps = 1,maxsteps=100,maxiter=2000)
            except:
                print 'failed to move up'
           
            self.waitrobot(self.robot) #waits for robot to finish the motion

        up_position = deepcopy(goal_config)
        up_position[0][3] = up_position[0][3]-0.01
        up_position[2][3] = up_position[2][3]+0.01

        try:
            self.basemanip.MoveToHandPosition(matrices=[up_position])
            self.waitrobot(self.robot)
        except planning_error,e:
            print 'failed to reach a goal,trying to move goal a little up',e
            manipulate = self.manipulator.GetTransform()
            print manipulate
            manipulate[0:3,3]+=np.array([0,0,1])*0.015
            print manipulate
            try:

                self.basemanip.MoveToHandPosition(matrices=[manipulate])
                self.waitrobot(self.robot)

            except planning_error,e:
                print e

I have put goal_config is same as the transformation matrix after it has grasped the object and later manipulated it in up_position.

When I am executing the code, I am consistently getting the following error:

move hand up
failed to move up
[basemanipulation.cpp:753] failed to find goal
failed to reach a goal,trying to move goal a little up openrave planning_error: 'MoveToHandPosition'
[[ 0.32875196  0.20462902  0.92198108  0.27220186]
 [-0.84839613 -0.36489854  0.38350106  0.36850414]
 [ 0.414905   -0.9082819   0.05364536  0.81090396]
 [ 0.          0.          0.          1.        ]]
[[ 0.32875196  0.20462902  0.92198108  0.27220186]
 [-0.84839613 -0.36489854  0.38350106  0.36850414]
 [ 0.414905   -0.9082819   0.05364536  0.82590396]
 [ 0.          0.          0.          1.        ]]
[basemanipulation.cpp:753] failed to find goal
openrave planning_error: 'MoveToHandPosition'


My questions:
1) Is there a possibility of self collision ? I have already added <adjacent> tags to avoid collision between the joints.
2) Is there a possibility with Inverse Kinematics when I am shifting my goal transformation matrix by a very small distance?
e.g in z direction manipulate[0:3,3]+=np.array([0,0,1])*0.015
3) Is environment locking necessary ? As per the example, environment is not locked when MoveToHandPosition is called.
4) This question is not related to this problem. I wanted to understand the concept of seedik in the taskmanipulator.GraspPlanning. I searched on openrave site, but found only examples of its usage.

Kindly guide me.

Thanking you.