Controlling actual robot using Openrave with a joystick to give the end effector goal positions.

classic Classic list List threaded Threaded
5 messages Options
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Controlling actual robot using Openrave with a joystick to give the end effector goal positions.

rvijay
This post was updated on .
Hi,

I have recently started working on openrave and got the simulated 6 DOF arm up and running (for the starters) using openravepy.
I have a driver code written for the physical arm, that includes the interface with all the motors at each joint.

My first question is -
  1. How can I get the joint angles, velocities and acceleration to be sent to each joint for following the planned trajectory (which I can see in the visualizer) in python? (so that I can send across the Motor driver code via sockets for moving the actual arm , the way arm in visualizer is moving.
 
  2. I want to control the arm using a 3 Axis joystick and its output is x,y,z. My python script (which I modified from couple of examples to suit my requirements) is getting this x,y,z over socket.
How can I use the xyz as an input for the goal points for the endeffector of the arm?

This is the initial layout of the code.
       
        manip = robot.SetActiveManipulator('arm')
        .......
        ikmodel =  databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D)
        .....
        sol = manip.FindIKSolution(..., IkFilterOptions.IgnoreSelfCollisions)
        ....
        manipprob = interfaces.BaseManipulation(robot)
        manipprob.MoveManipulator(sol)



Thanks,
Ritukar Vijay
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: How to get the joint angles as an input to the motors from openravepy

Rosen Diankov
Administrator
1. each planner produces an openrave Trajectory. Each trajectory has a Sample method you can use the get the joint values of the robot at each time step. for example

http://openrave.org/docs/latest_stable/tutorials/openravepy_examples/#returning-a-trajectory

2. you should take a look at the python and c++ IK examples:

openrave.org/docs/latest_stable/tutorials/openravepy_examples/#returning-a-trajectory




2014-06-26 15:49 GMT+09:00 rvijay <[hidden email]>:
Hi,

I have recently started working on openrave and got the simulated 6 DOF arm
up and running (for the starters) using openravepy.
I have a driver code written for the physical arm, that includes the
interface with all the motors at each joint.

My first question is -
  1. How can I get the joint angles, velocities and acceleration to be sent
to each joint for following the planned trajectory (which I can see in the
visualizer) in python? (so that I can send across the Motor driver code via
sockets for moving the actual arm , the way arm in visualizer is moving.

  2. I want to control the arm using a 3 Axis joystick and its output is
x,y,z. My python script (which I modified from couple of examples to suit my
requirements) is getting this x,y,z over socket.
How can I use the xyz as an input for the goal points for the endeffector of
the arm?

This is the initial layout of the code.
       /
        manip = robot.SetActiveManipulator('arm')
        .......
        ikmodel =
databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D)
        .....
        sol = manip.FindIKSolution(...,
IkFilterOptions.IgnoreSelfCollisions)
        ....
        manipprob = interfaces.BaseManipulation(robot)
        manipprob.MoveManipulator(sol)
/


Thanks,
Ritukar Vijay



--
View this message in context: http://openrave-users-list.185357.n3.nabble.com/How-to-get-the-joint-angles-as-an-input-to-the-motors-from-openravepy-tp4027036.html
Sent from the OpenRAVE Users List mailing list archive at Nabble.com.

------------------------------------------------------------------------------
Open source business process management suite built on Java and Eclipse
Turn processes into business applications with Bonita BPM Community Edition
Quickly connect people, data, and systems into organized workflows
Winner of BOSSIE, CODIE, OW2 and Gartner awards
http://p.sf.net/sfu/Bonitasoft
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users


------------------------------------------------------------------------------
Open source business process management suite built on Java and Eclipse
Turn processes into business applications with Bonita BPM Community Edition
Quickly connect people, data, and systems into organized workflows
Winner of BOSSIE, CODIE, OW2 and Gartner awards
http://p.sf.net/sfu/Bonitasoft
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: How to get the joint angles as an input to the motors from openravepy

rvijay
Hi Rosen,
Thanks, I got the trajectories using the following.
traj = manipprob.MoveManipulator(sol, execute=False,outputtrajobj=True)

I performed the following steps:

1. I imported the collada files in the kinbody.xml. I spent some time to set the anchor values for each joints.
2.Converted the angles received from the joystick into radians (alpha, beta, gamma)
3. Calculated the rotational matrices for each alpha, beta and gamma.
                 Rotz = numpy.array([[math.cos(alpha), -math.sin(alpha),  0,  0],
                                         [math.sin(alpha),  math.cos(alpha), 0, 0],
                                         [0, 0, 1, 0],
                                         [ 0.        ,  0.        ,  0.        ,  1.        ]])
                Rotx = numpy.array([[1, 0, 0, 0],
                [math.cos(theta), 0, -math.sin(theta),  0],
                                         [math.sin(theta), 0, math.cos(theta), 0],
                                         [ 0.        ,  0.        ,  0.        ,  1.        ]])
                Roty = numpy.array([[math.cos(theta), 0, math.sin(theta),  0],
                                         [0, 1, 0, 0],
                                         [-math.sin(theta), 0, math.cos(theta), 0],
                                         [ 0.        ,  0.        ,  0.        ,  1.        ]])

4. Calculated the homogenous transformation matrix -
                Teerotated=numpy.dot(Tee,Rotz)        
                Teerotated=numpy.dot(Teerotated,Roty)
                Teerotated=numpy.dot(Teerotated,Rotx)

4. sol = manip.FindIKSolution(Teerotated, IkFilterOptions.IgnoreSelfCollisions)
5. manipprob = interfaces.BaseManipulation(robot)
And then did the following,

try :
    traj = manipprob.MoveManipulator(sol, execute=False,outputtrajobj=True)
                        robot.GetController().SetPath(traj)
                        print "waiting for controller"
# robot.WaitForController(0)
                        print "Controller returned"
                except :
    pass

Now the issues, which I am facing are -

1.  I am getting a log while starting my python script.
"manipulator arm gripper dof 5 is also part of arm dof! excluding from gripper..."
Will it disrupts the proper functioning of the planner.

2. For a certain values coming from the joystick, the py code crashes.
3. I want to fix a pivot point of the end effector and add this as a constraint - Haven't reached there !!


Please suggest about the Issue 1 and Issue 2. I think for Issue 3, I will raise a separate topic under constraint planning.

Thanks,
R. Vijay
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: How to get the joint angles as an input to the motors from openravepy

rvijay
Hi,

I figured out the problem, with this one. The problem was with the angles which I was getting from the joystick.

Open Question -
What I am trying to do -
>I am trying to keep the x,y,z of the robot end effector same w.r.t base and only send various angles, so that the end effector extensions moves only in a cone.

Solution - Attempt 1
>> The homogeneous transformation matrix which I am sending to  FindIKSolution, contains the same translational component and the rotational matrix changes as per the angles provided by the 3 Axis Joystick.

Although the pivot point remains the same, but I guess to achieve the stable pivot point, some constraints need to be added.

Any suggestions on this will be deeply appreciated.


Thanks,
R. Vijay
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: How to get the joint angles as an input to the motors from openravepy

rvijay
Thanks it worked perfectly after playing around with the transformation and rotational matrix.
Loading...