Joint trajectory format


Jose_UTEC
 Share

Recommended Posts

I have tried to use .set_joint_trajectory. It's working with one joint but I have problem with multiple joint. The error msg is attached.

 

def set_j():
    limb = intera_interface.Limb('right')
    limb.move_to_neutral()
    names=["right_j6","right_j5"]
    positions=[[-2.42577,-2.46446,-2.49639,-2.52226,-2.54279,-2.55868,-2.57063,-2.57935,-2.58555,-2.58992,-2.59318,-2.59602,-2.59916,-2.6033,-2.60914,-2.6174,-2.62876,-2.64395,-2.66367,-2.68861,-2.71949,-2.75664,-2.79891,-2.8448,-2.89279,-2.94136,-2.989,-3.0342,-3.07544,-3.11121,-3.14,-3.16072,-3.174,-3.1809,-3.1825,-3.17984,-3.174,-3.16603,-3.157,-3.14797,-3.14,-3.13394,-3.1298,-3.12736,-3.1264,-3.12672,-3.1281,-3.13033,-3.1332,-3.13649,-3.14,-3.14351,-3.1468,-3.14967,-3.1519,-3.15328,-3.1536,-3.15264,-3.1502,-3.14606,-3.14],[0.91,0.862531,0.815121,0.768189,0.722156,0.677442,0.634467,0.593652,0.555416,0.520181,0.488365,0.460391,0.436677,0.417644,0.403712,0.395302,0.392834,0.396728,0.407404,0.425283,0.450785,0.484025,0.523899,0.568998,0.617913,0.669234,0.721554,0.773462,0.82355,0.870408,0.912628,0.948976,0.978928,1.00213,1.01824,1.0269,1.02776,1.02047,1.00468,0.980052,0.946223,0.903247,0.852784,0.796894,0.737636,0.677072,0.617261,0.560263,0.508139,0.462949,0.426753,0.401611,0.389584,0.392731,0.413112,0.452789,0.51382,0.598267,0.70819,0.845648,1.0127]]
    
    velocities=[[-0.386873,-0.319299,-0.258777,-0.205306,-0.158887,-0.119519,-0.0872025,-0.0619379,-0.0437248,-0.0325633,-0.0284533,-0.0313949,-0.0413881,-0.0584328,-0.0825292,-0.113677,-0.151876,-0.197127,-0.24943,-0.308784,-0.371491,-0.422757,-0.458884,-0.479871,-0.485718,-0.476426,-0.451994,-0.412422,-0.357711,-0.28786,-0.207164,-0.132797,-0.0690547,-0.0159357,0.0265595,0.0584309,0.0796785,0.0903023,0.0903023,0.0796785,0.0605556,0.0414328,0.0244347,0.00956141,-0.00318714,-0.0138109,-0.02231,-0.0286842,-0.0329338,-0.0350585,-0.0350585,-0.0329338,-0.0286842,-0.02231,-0.0138109,-0.00318714,0.00956141,0.0244347,0.0414328,0.0605556,0],[-0.474686,-0.474104,-0.469319,-0.460331,-0.447141,-0.429749,-0.408153,-0.382356,-0.352355,-0.318153,-0.279747,-0.237139,-0.190329,-0.139315,-0.0840997,-0.0246815,0.0389393,0.106763,0.178789,0.255017,0.332399,0.39874,0.450989,0.489148,0.513217,0.523194,0.519081,0.500877,0.468583,0.422198,0.363487,0.299511,0.232035,0.161059,0.086583,0.00860702,-0.072869,-0.157845,-0.246321,-0.338297,-0.429756,-0.504631,-0.558903,-0.592574,-0.605643,-0.59811,-0.569976,-0.521239,-0.451901,-0.361961,-0.251419,-0.120276,0.0314695,0.203817,0.396766,0.610316,0.844469,1.09922,1.37458,1.67054,1.67054]]
    
    accelerations=[[0.67574,0.605225,0.534709,0.464193,0.393678,0.323162,0.252647,0.182131,0.111615,0.0410995,-0.0294161,-0.0999318,-0.170447,-0.240963,-0.311479,-0.381994,-0.45251,-0.523026,-0.593541,-0.627072,-0.512661,-0.361265,-0.209869,-0.0584728,0.0929232,0.244319,0.395715,0.547111,0.698507,0.806965,0.743666,0.637428,0.53119,0.424952,0.318714,0.212476,0.106238,4.44089e-14,-0.106238,-0.191228,-0.191228,-0.169981,-0.148733,-0.127486,-0.106238,-0.0849904,-0.0637428,-0.0424952,-0.0212476,0,0.0212476,0.0424952,0.0637428,0.0849904,0.106238,0.127486,0.148733,0.169981,0.191228,-0.605556,0],[0.00582343,0.0478491,0.0898747,0.1319,0.173926,0.215952,0.257977,0.300003,0.342029,0.384054,0.42608,0.468106,0.510131,0.552157,0.594183,0.636208,0.678234,0.72026,0.762285,0.773822,0.663404,0.522497,0.38159,0.240683,0.0997759,-0.0411311,-0.182038,-0.322945,-0.463852,-0.587108,-0.639759,-0.67476,-0.70976,-0.74476,-0.77976,-0.81476,-0.849761,-0.884761,-0.919761,-0.914592,-0.748743,-0.542726,-0.336708,-0.13069,0.0753281,0.281346,0.487364,0.693382,0.8994,1.10542,1.31144,1.51745,1.72347,1.92949,2.13551,2.34152,2.54754,2.75356,2.95958,2.95958,2.95958]]


    limb.set_joint_trajectory(names, positions, velocities, accelerations)

Screenshot from 2018-07-05 12-17-04.png

Link to comment
Share on other sites

  • 2 weeks later...

Hi Jose_UTEC,

I think that with the joint trajectory control you should send one set of (position, velocity, acceleration) at the time.
In your case you want to control "right_j6" and "right_j5". Lets say the trajectory right_j6 has to follow is an array traj_j6 = the huge array you gave for j6 angles (around 60 values according to what i can see in your post). Let's say you want your joint to be at one of this angle values every 0.1 sec (10 Hz), so with 60 values you are going to last 6 sec. If you call vel_j6 = the huge array you gave for j6 velocities and acc_j6 = the huge array you gave for j6 accelerations (and same for j5 with traj_j5, vel_j5 and acc_j5), then you should have a code like:

import rospy
import intera_interface

limb = intera_interface.Limb('right')
limb.move_to_neutral()
# control rate, data sent every 0.1 s
rate = 10 # Hz
control_rate = rospy.Rate(rate)
# controlled joints
names=['right_j6','right_j5']
i = 0 # initializing time step
# looping over your arrays of 60 values for pose, vel and acc for each joint
while i < len(traj_j6):
	pose = [traj_j6[i], traj_j5[i]]
  	velocity = [vel_j6[i], vel_j5[i]]
    acceleration = [acc_j6[i], acc_j5[i]]
    # sending targeted pose, vel and acceleration for current time step  
    limb.set_joint_trajectory(names, pose, velocity, acceleration)
    # incrementing time step
    i += 1
    # wait around 0.1 s before looping to next time step
    control_rate.sleep()
    
print "Trajectory done..."

Might not be the perfect way to do it but it worked for me. If any one has any better suggestions, I would be happy to see how it can be improved...
hope that it is what you tried to do and that this can help,

Good luck
 

Link to comment
Share on other sites

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

 Share