Drive gen3 using PreComputedJointTrajectory

ch1hha0 commented 1 year ago

Excuse me, does release 2.3.0 support using PreComputedJointTrajectory to drive gen3 7dof in ROS? I want to control the robotic arm in joint space, and I have written a program using PreComputedJointTrajectory to drive gen3. However, gen3 did not move, and there was no error message when the program was running. The following is part of the code:

    def FillPreComputedJointTrajectory(self,point):
        precomputedjointWaypoint = kortex_driver.msg.PreComputedJointTrajectoryElement()

        precomputedjointWaypoint.joint_angles = point.positions
        precomputedjointWaypoint.joint_speeds = point.velocities
        precomputedjointWaypoint.joint_accelerations = point.accelerations
        precomputedjointWaypoint.time_from_start = point.time_from_start.secs + point.time_from_start.nsecs / 1000000000

        #precomputedjointWaypoint.time_from_start.sec = point.time_from_start.secs
        #precomputedjointWaypoint.time_from_start.usec = point.time_from_start.nsecs / 1000

        return precomputedjointWaypoint

    def example_send_pre_computed_joint_trajectory(self,plan):
        self.last_action_notif_type = None
        #req = kortex_driver.srv.PlayPreComputedJointTrajectoryRequest()
        req = kortex_driver.srv.ExecuteActionRequest()
        goal = kortex_driver.msg.PreComputedJointTrajectory()
        #goal.mode = 0
        for point in plan.joint_trajectory.points:
            #req.input.trajectory_elements.append(self.FillPreComputedJointTrajectory(point))
            goal.trajectory_elements.append(self.FillPreComputedJointTrajectory(point))

        req.input.oneof_action_parameters.play_pre_computed_trajectory.append(goal)

        #print(req.input.trajectory_elements)
        rospy.loginfo("Sending the PreComputedJointTrajectory...")
        try:
            #self.play_pre_computed_joint_trajectory(req)
            self.execute_action(req)
        except rospy.ServiceException:
            rospy.logerr("Failed to call PlayPreComputedJointTrajectory")
            return False
        else:
            return self.wait_for_action_end_or_abort()

The plan is computed using MoveIt to generate a path. If this control method works well, I plan to explore other algorithms to generate paths in the future.

felixmaisonneuve commented 1 year ago

Hi @ch1hha0,

You can refer to the old joint trajectory server that was using PlayPreComputedJointTrajectory to execute MoveIT trajectories https://github.com/Kinovarobotics/ros_kortex/blob/0b422bb0770b2383eac4dec2d1566b3d65c3c31f/kortex_driver/src/non-generated/driver/pre_computed_joint_trajectory_action_server.cpp#L91-L131

Quickly looking at your code, I think you are missing a mode to your PreComputedJointTrajectory

goal = kortex_driver.msg.PreComputedJointTrajectory()
goal.mode = kortex_driver.msg.TrajectoryContinuityMode.TRAJECTORY_CONTINUITY_MODE_POSITION # not sure if this is the correct syntax

If you don't specify this field, it will defaults to 0, which is "TRAJECTORY_CONTINUITY_MODE_UNSPECIFIED" It might explain why it is not working

Regards, Felix

ch1hha0 commented 1 year ago

Hi @felixmaisonneuve , I tried your suggestion, goal.mode = kortex_driver.msg.TrajectoryContinuityMode.TRAJECTORY_CONTINUITY_MODE_POSITION, but the robotic arm still cannot move and there are no error messages. I wonder if there are other ways to execute joint trajectories continuously, such as FollowJointTrajectoryAction, but it seems to be disabled. I also tried Waypointlist, but this method did not meet my expectations.

felixmaisonneuve commented 1 year ago

You can use PlayJointTrajectory. The fonction is deprecated and will maybe be removed in the future, but it is still working at the moment. You can also do a reach_joint_angles action. I think these are your only 2 options other than waypoint lists and PlayPreComputedJointTrajectory.

I am not sure why your PreComputedJointTrajectory action does not work. What des your trajectory look like? You should have a trajectory element for every ms. The trajectory has to be less than 30s (30 000 trajectory elements) if I am not mistaken. Also, your first trajectory element should be the starting point of the arm.

ch1hha0 commented 1 year ago

The trajectory I am currently using for testing is shown below. In simple terms, I am trying to use the robotic arm to perform the task of writing.

[positions: [7.410547998354389e-06, 0.2601718368963022, 3.1400228969698714, -2.2701010041961975, -3.1287399238522084e-05, 0.9597233990196008, 1.5697763787858365]
velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
accelerations: [-0.02918398815095891, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
effort: []
time_from_start: 
  secs: 0
  nsecs:         0, positions: [-0.001767933568192587, 0.260203081337934, 3.1403335499175133, -2.270120268050883, -0.0036417485157495314, 0.9595270069079064, 1.5732141880022623]
velocities: [0.00469403990733471, 0.03897158291989309, 0.0020930239964484904, -0.0016976423044843271, 0.005483072362160208, 0.04031626345688822, -0.0014617296767829874]
accelerations: [0.053545272804457145, 0.21279507965251795, 0.006580577235939805, -0.008988673865472242, 0.08665690674839718, 0.22372597056381188, -0.06193978823672886]
effort: []
time_from_start: 
  secs: 0
  nsecs: 348806003, positions: [0.003763702984397301, 0.28994904635449564, 3.141592653589793, -2.271396422519474, 0.0045029818031776985, 0.9905498603442838, 1.5683314920472553]
velocities: [0.022079406569185674, 0.1083744621356984, 0.0016477149346538855, -0.0022973701371706177, 0.02786756521510096, 0.11073440429430251, -0.015118131527508814]
accelerations: [0.05086052667876921, 0.20420930955888741, -0.011024545182339753, 0.0069764156285580395, 0.04382818142763983, 0.19763869858703123, -0.015648044113166917]
effort: []
time_from_start: 
  secs: 0
  nsecs: 730881702, positions: [0.010167647836953507, 0.31991701613054674, 3.141592653589793, -2.2716671322751147, 0.011929003172292932, 1.020815126411263, 1.5645650090020007]
velocities: [0.03304814957586543, 0.15737392226093003, 0.0, 0.001429539026842458, 0.03759316977550625, 0.15613904009160323, -0.01798568084139683]
accelerations: [0.03481079890704677, 0.19103589709137941, 0.0, 0.02775011146238188, 0.03282489943457213, 0.16402428439631309, -0.00546697933454536]
effort: []
time_from_start: 
  secs: 0
  nsecs: 946641073, positions: [0.016400281173262605, 0.3500148867430903, 3.141592653589793, -2.270963045269504, 0.01890665769353381, 1.0502544525089772, 1.5613961784537491]
velocities: [0.03895971177843781, 0.1911023463821344, 0.0, 0.007786108905702407, 0.042913304120413304, 0.18364874350856922, -0.018288783440643448]
accelerations: [0.03206926662653067, 0.1922090295879533, 0.0, 0.046286288227102165, 0.027036044840147603, 0.14675984152925942, 0.0028448418616559873]
effort: []
time_from_start: 
  secs: 1
  nsecs: 117795129, positions: [0.02246839992268003, 0.38018467888286916, 3.141592653589793, -2.269287757732766, 0.02549442785722757, 1.0788072815582974, 1.5587552604568629]
velocities: [0.04373903908671848, 0.2206001069611367, 0.0, 0.016007864060829987, 0.04683039051138613, 0.20505065125027022, -0.017490815792792862]
accelerations: [0.032538320241834395, 0.20743291686091547, 0.0, 0.06623399346117247, 0.025798959103924164, 0.14206498245745597, 0.008331403585530153]
effort: []
time_from_start: 
  secs: 1
  nsecs: 264000475, positions: [0.02837800170758121, 0.4103725781705975, 3.141592653589793, -2.2666452793073733, 0.03174189600446861, 1.1064192738540501, 1.5565805106618176]
velocities: [0.047504123628528946, 0.24584893278654663, 0.0, 0.025624630688364958, 0.049639984122069256, 0.22078631133010518, -0.015971377562222083]
accelerations: [0.024885567461499485, 0.17891552356929186, 0.0, 0.08241407983747955, 0.016874303869879117, 0.09721890073341165, 0.015404664625368]
effort: []
time_from_start: 
  secs: 1
  nsecs: 392542746, positions: [0.03413481844643665, 0.4405277731894516, 3.141592653589793, -2.2630419173308445, 0.03769163983181865, 1.1330422681776098, 1.5548166099281637]
velocities: [0.050198389306180084, 0.2661138103351389, 0.0, 0.03620298457548662, 0.051381810956827145, 0.23054734920987824, -0.013934445034906076]
accelerations: [0.020548010355091424, 0.16351699658224417, 0.0, 0.09727371144126035, 0.012431203755840975, 0.06677987521322942, 0.019235052347891763]
effort: []
time_from_start: 
  secs: 1
  nsecs: 509946735, positions: [0.039744329696457234, 0.4706029055141752, 3.141592653589793, -2.2584861633026145, 0.04338017488464592, 1.1586344839342804, 1.5534137886313983]
velocities: [0.05236168323746018, 0.283848428394917, 0.0, 0.04768465379331538, 0.052680235952870726, 0.23684971431662977, -0.01172523875266328]
accelerations: [0.01888087581698637, 0.16006817708274826, 0.0, 0.11282674485180523, 0.011227377505870351, 0.04760077438326669, 0.021154113117187373]
effort: []
time_from_start: 
  secs: 1
  nsecs: 619160817, positions: [0.045211775103939617, 0.5005544311928128, 3.141592653589793, -2.2529885441122075, 0.04883876119695438, 1.1831606214254364, 1.5523271012506643]
velocities: [0.054168541832349565, 0.29977467505635125, 0.0, 0.05997137483118589, 0.0537382720619437, 0.24051613179604658, -0.009486544945992319]
accelerations: [0.016201041777102528, 0.14954590808632795, 0.0, 0.12668843972972427, 0.009304891066915852, 0.02301544346949401, 0.022449925755247208]
effort: []
time_from_start: 
  secs: 1
  nsecs: 721622575, positions: [0.05054216621286735, 0.5303428894739095, 3.141592653589793, -2.2465614429186216, 0.05409409927745623, 1.2065918569204706, 1.5515158234367885]
velocities: [0.055850354193010446, 0.3150661962496869, 0.0, 0.07320638394926571, 0.054790584227746006, 0.24258156616443, -0.007309389085047071]
accelerations: [0.018539279291000726, 0.16619444021092206, 0.0, 0.14674647232999158, 0.012479927924915224, 0.01947016237815373, 0.022437671750923503]
effort: []
time_from_start: 
  secs: 1
  nsecs: 818580655, positions: [0.055740297664341805, 0.5599330785919437, 3.141592653589793, -2.2392188952788534, 0.05916893484904488, 1.2289057345650016, 1.5509429492680786]
velocities: [0.057375856144847, 0.32945767254100844, 0.0, 0.08722715528035971, 0.05580585853124195, 0.24294492819970662, -0.0052324890722268294]
accelerations: [0.014554702925205327, 0.1464774684545005, 0.0, 0.1586828575263015, 0.009537697064807534, -0.012392841308927231, 0.02276818956430379]
effort: []
time_from_start: 
  secs: 1
  nsecs: 910218920, positions: [0.060810757823648316, 0.5892941420282283, 3.141592653589793, -2.2309763665899838, 0.0640825875556915, 1.2500859619539482, 1.5505747703720156]
velocities: [0.05872998787832262, 0.34283497517144035, 0.0, 0.10198096553732397, 0.056761403541548634, 0.24154686800940367, -0.003273550040211296]
accelerations: [0.01647490380918041, 0.15992767051763726, 0.0, 0.17939210186797896, 0.012394461842336219, -0.019773026717849914, 0.022036125751167236]
effort: []
time_from_start: 
  secs: 1
  nsecs: 997599596, positions: [0.06575793885166359, 0.618399571729862, 3.141592653589793, -2.2218505170325806, 0.06885141551098499, 1.2701221199343409, 1.5503805218689155]
velocities: [0.059884255451925994, 0.3549541916031427, 0.0, 0.11724761471279971, 0.05762388222917154, 0.23836616394162297, -0.0014694643889415924]
accelerations: [0.011062744930874767, 0.12979182214536297, 0.0, 0.18656420827011003, 0.008178107737982264, -0.05726504951330168, 0.02117181198282957]
effort: []
time_from_start: 
  secs: 2
  nsecs:  80839609, positions: [0.07058604625907394, 0.6472271371387215, 3.141592653589793, -2.21185896003769, 0.07348922504461769, 1.2890092998543117, 1.5503320827024343]
velocities: [0.06082399262664695, 0.3657096585629096, 0.0, 0.13293299739985504, 0.058368812550019085, 0.23339372096196775, 0.00016329880310540377]
accelerations: [0.012447137939296403, 0.13915418577621955, 0.0, 0.205767306561819, 0.01048220272723802, -0.06718470708838895, 0.019596654274965597]
effort: []
time_from_start: 
  secs: 2
  nsecs: 160860239, positions: [0.07529910796757017, 0.6757587505174514, 3.141592653589793, -2.2010200198753003, 0.07800763299574633, 1.3067476830294074, 1.55040372013438]
velocities: [0.06122049307731221, 0.37302348968124377, 0.0, 0.14799437958236877, 0.05867171446776209, 0.22559470170834495, 0.0015827709524650498]
accelerations: [-0.0024109916523884986, 0.04878921423586472, 0.0, 0.18378121331794633, -0.0028455219221447305, -0.13575568709070757, 0.017110308874958655]
effort: []
time_from_start: 
  secs: 2
  nsecs: 237730121, positions: [0.07990098289551613, 0.703980279974848, 3.141592653589793, -2.189352493324838, 0.08241638739697675, 1.3233420777368037, 1.5505718700132776]
velocities: [0.06136463999644075, 0.3786468651841449, 0.0, 0.16299850844370956, 0.058799628132715165, 0.2161591084125481, 0.002782616433229874]
accelerations: [0.006364084262490243, 0.10165919746975138, 0.0, 0.2162272100129122, 0.006372053830629829, -0.11526392566878335, 0.014813780530537788]
effort: []
time_from_start: 
  secs: 2
  nsecs: 313011758, positions: [0.08439536910087223, 0.7318813218281736, 3.141592653589793, -2.176875419631938, 0.08672365130209578, 1.3388014287481242, 1.550814945916085]
velocities: [0.06154989165977156, 0.38432647444207513, 0.0, 0.17842890082150262, 0.05902198284341373, 0.20635591289731836, 0.0037545144801314698]
accelerations: [-0.0014022622524572145, 0.05298419715514142, 0.0, 0.20552525316358736, -0.0003823079993639202, -0.15327874811083853, 0.011718500148370004]
effort: []
time_from_start: 
  secs: 2
  nsecs: 385971985, positions: [0.08878581150149949, 0.7594549436207572, 3.141592653589793, -2.1636078621108887, 0.09093625365828506, 1.35313831340764, 1.5511131715577575]
velocities: [0.060859229558699234, 0.38432647444207513, 0.0, 0.19145981074952, 0.05844688140127753, 0.193270742462205, 0.004441801728193661]
accelerations: [-0.017950385850424386, -0.05362395163553728, 0.0, 0.1574442778883503, -0.01574176332345251, -0.21184336306112378, 0.007414937602534606]
effort: []
time_from_start: 
  secs: 2
  nsecs: 457362120, positions: [0.09307570919218475, 0.7866974083527202, 3.141592653589793, -2.149568703881873, 0.09505991052682358, 1.3663684367556692, 1.551448431925774]
velocities: [0.05960443681732561, 0.3805212618238368, 0.0, 0.20259969957796892, 0.05736055792362807, 0.17827484379147546, 0.004855485418112337]
accelerations: [-0.017278546420714053, -0.053211095559991546, 0.0, 0.1553179221339643, -0.014756862401334147, -0.2091799338044117, 0.0041961091920722626]
effort: []
time_from_start: 
  secs: 2
  nsecs: 528600192, positions: [0.09726832238218326, 0.8136078893976364, 3.141592653589793, -2.1347764594032737, 0.09909941952699078, 1.378510136388406, 1.5518041394822255]
velocities: [0.05839646129165735, 0.3767537245780562, 0.0, 0.2135590414910219, 0.056338851183825214, 0.1635066426977391, 0.005047906808650509]
accelerations: [-0.016713681241978667, -0.05280836634257686, 0.0, 0.15307950765886105, -0.013993456965451737, -0.20639960915879202, 0.001215195081422818]
effort: []
time_from_start: 
  secs: 2
  nsecs: 599673838, positions: [0.10136677897359024, 0.8401881843137594, 3.141592653589793, -2.119249102669813, 0.10305883006822941, 1.3895839057604782, 1.5521651125114881]
velocities: [0.05722808702997251, 0.3730234896812437, 0.0, 0.22432887660818013, 0.055367218956111186, 0.14897565559461223, 0.0050361872808317425]
accelerations: [-0.0162423758791401, -0.05241081433104376, 0.0, 0.15070504801347226, -0.013412918308284979, -0.20347603175473739, -0.0015490790456486872]
effort: []
time_from_start: 
  secs: 2
  nsecs: 670577440, positions: [0.1053740807962378, 0.8664424343946354, 3.141592653589793, -2.103003912268555, 0.10694159168849557, 1.3996119426586617, 1.5525174633174152]
velocities: [0.056092944639251005, 0.3693301878032116, 0.0, 0.2348997910733818, 0.05443369227861949, 0.13469167047386815, 0.004836035780919086]
accelerations: [-0.015852430678991246, -0.052013945779748647, 0.0, 0.1481763327427866, -0.012981312476339678, -0.20038886605968967, -0.004113060020432292]
effort: []
time_from_start: 
  secs: 2
  nsecs: 741311649, positions: [0.10929310952141139, 0.8923768554595246, 3.141592653589793, -2.086057332879082, 0.11075068267138398, 1.4086177276650553, 1.5528484944450647]
velocities: [0.05498544115802098, 0.3656734532705065, 0.0, 0.2452621964370605, 0.05352856440017777, 0.12066448614021716, 0.004462003218869111]
accelerations: [-0.015532724152307224, -0.05161370792617763, 0.0, 0.1454805871770784, -0.01266873365845497, -0.19712349013905361, -0.006489282888675645]
effort: []
time_from_start: 
  secs: 2
  nsecs: 811882899, positions: [0.11312663226950688, 0.917999484097409, 3.141592653589793, -2.0684248523271744, 0.11448872095354344, 1.4166256357041684, 1.5531466015257267]
velocities: [0.053900693893682045, 0.36205292403020445, 0.0, 0.25540657782025056, 0.05264411574754335, 0.10690367994841868, 0.003927678744645266]
accelerations: [-0.015273104724978595, -0.05120647270024311, 0.0, 0.14261010473294394, -0.012448789700290092, -0.19367064163118997, -0.008687402541918052]
effort: []
time_from_start: 
  secs: 2
  nsecs: 882302947, positions: [0.11687730692742895, 0.943319942421138, 3.141592653589793, -2.050120892905099, 0.11815805922733837, 1.423660582239055, 1.5534011816460378]
velocities: [0.05206531947753634, 0.35322321775411003, 0.0, 0.2613500817215659, 0.05101938081835458, 0.09214281512487615, 0.003203641035268155]
accelerations: [-0.03641710954468986, -0.19717390321077827, 0.0, 0.0260056785850307, -0.03329500565907627, -0.22297726248858793, -0.01173896406352729]
effort: []
time_from_start: 
  secs: 2
  nsecs: 952588418, positions: [0.12054768719445062, 0.9683492233720644, 3.141592653589793, -2.0311587153812853, 0.12176086604096546, 1.4297477044014582, 1.5536025463882124]
velocities: [0.04977001606137294, 0.3411040013224076, 0.0, 0.2642416221586133, 0.04893469947448319, 0.07715341349254956, 0.0023381247005391627]
accelerations: [-0.027328110444627837, -0.13951430003765813, 0.0, 0.053839011682768126, -0.024605888554585644, -0.19296570753586859, -0.012252110415772934]
effort: []
time_from_start: 
  secs: 3
  nsecs:  24886683, positions: [0.12414022736675362, 0.993099497762173, 3.141592653589793, -2.0115503339061944, 0.1252991945794553, 1.4349120762880934, 1.5537418388869844]
velocities: [0.047809912640670235, 0.3310721831022271, 0.0, 0.2679267208252214, 0.04716744066771687, 0.06347007000442549, 0.0014058340636195399]
accelerations: [-0.025896467198246367, -0.13288277466087317, 0.0, 0.046271971678805236, -0.023381566975547666, -0.17863220244812567, -0.013049345488100961]
effort: []
time_from_start: 
  secs: 3
  nsecs:  98545313, positions: [0.12765728688080605, 1.0175839435512013, 3.141592653589793, -1.9913064398709437, 0.12877504073332383, 1.4391784568352175, 1.5538109543658092]
velocities: [0.045916170345906164, 0.3213353991719188, 0.0, 0.27113179469992, 0.045455572374973524, 0.05057198347574275, 0.00040485211024345047]
accelerations: [-0.024554991637121003, -0.12651120702348662, 0.0, 0.039161528883638656, -0.022224029277377797, -0.16503061496718213, -0.013605755352565871]
effort: []
time_from_start: 
  secs: 3
  nsecs: 173620776, positions: [0.13110113462044634, 1.041816597319288, 3.141592653589793, -1.9704363326745014, 0.13219039193060314, 1.4425710680561685, 1.5538024637259438]
velocities: [0.043870988644796494, 0.31036403063519774, 0.0, 0.27251098671417795, 0.04358382216536436, 0.038272390260687295, -0.0006444446942472443]
accelerations: [-0.02867649525438877, -0.1589211233620513, 0.0, -0.00271043794113801, -0.02648733163275478, -0.15552550124222406, -0.013729773337523878]
effort: []
time_from_start: 
  secs: 3
  nsecs: 250175648, positions: [0.1344739530130806, 1.065812227504542, 3.141592653589793, -1.9489478552957586, 0.1355472681496505, 1.4451134009992204, 1.553709539810178]
velocities: [0.04169020229560981, 0.29825373307488773, 0.0, 0.27207403239707817, 0.04156824734321875, 0.02665577662784166, -0.0017180703274114312]
accelerations: [-0.026625173026413027, -0.14816928248701577, 0.0, -0.00827965978391202, -0.02462430765237557, -0.13913803529918262, -0.013484298225780308]
effort: []
time_from_start: 
  secs: 3
  nsecs: 329059756, positions: [0.13777784190923667, 1.0895862287079134, 3.141592653589793, -1.9268473325114837, 0.1388477563764112, 1.4468280464606245, 1.5535258860043408]
velocities: [0.039602825779571194, 0.2866159751535914, 0.0, 0.2711995443651233, 0.039637360577991165, 0.015955675974678937, -0.002799059606489361]
accelerations: [-0.024716194812086045, -0.138066112756617, 0.0, -0.013142103112490412, -0.022867873856206655, -0.12412352253096673, -0.013094617088485727]
effort: []
time_from_start: 
  secs: 3
  nsecs: 410388700, positions: [0.14101482227377451, 1.1131545362164323, 3.141592653589793, -1.904139509570102, 0.14209403974328993, 1.4477365473349224, 1.5532456668323789]
velocities: [0.03742286499488656, 0.27409592370260116, 0.0, 0.2685848212034732, 0.037606151450111126, 0.006115759123186326, -0.003854868283531587]
accelerations: [-0.027052874831000825, -0.15911716503785892, 0.0, -0.048360126466127876, -0.025362886866460484, -0.11000204466875312, -0.012018698248500782]
effort: []
time_from_start: 
  secs: 3
  nsecs: 494287692, positions: [0.14418683969038235, 1.1365335598351396, 3.141592653589793, -1.8808274890742454, 0.14528842245129805, 1.4478592693691374, 1.5528634402019448]
velocities: [0.034994123256761295, 0.25950725039658984, 0.0, 0.2629656453795064, 0.03531723052213315, -0.002791324235654715, -0.0048390202204208746]
accelerations: [-0.02827017291456393, -0.17303470260000098, 0.0, -0.07897975924330149, -0.02676913970107931, -0.09338548050114184, -0.010447527020305622]
effort: []
time_from_start: 
  secs: 3
  nsecs: 581758187, positions: [0.14729576768905317, 1.1597401361407167, 3.141592653589793, -1.8569126637550961, 0.14843335151693676, 1.447215287058098, 1.552374090924868]
velocities: [0.032511147342805, 0.24423124747559807, 0.0, 0.25560360468928356, 0.03296541590030347, -0.010645595376623506, -0.0057418022179805475]
accelerations: [-0.025601032736850552, -0.15835030735944605, 0.0, -0.08049576266419803, -0.024255959512996885, -0.07722946686542606, -0.009148255486110679]
effort: []
time_from_start: 
  secs: 3
  nsecs: 673944522, positions: [0.150343410910337, 1.1827914983633196, 3.141592653589793, -1.832394642710686, 0.15153143633236563, 1.4458222814178794, 1.5517727650749464]
velocities: [0.02990599471647609, 0.2276914256991354, 0.0, 0.24576552143815122, 0.030479587478868732, -0.01729571605984939, -0.006513259544432796]
accelerations: [-0.027539511380679595, -0.1787718934554175, 0.0, -0.11917464585173436, -0.026439825082185495, -0.05916206432830407, -0.006685845282347878]
effort: []
time_from_start: 
  secs: 3
  nsecs: 771319836, positions: [0.1533315081088842, 1.2057052632527412, 3.141592653589793, -1.807271168520959, 0.1545854669533922, 1.4436964463756294, 1.5510548046989419]
velocities: [0.026930642518691476, 0.20791560124332312, 0.0, 0.23117017457716335, 0.02760308219889067, -0.022443806064763983, -0.007054898469836046]
accelerations: [-0.028737975861761685, -0.19492596428546424, 0.0, -0.15557343048418487, -0.027952734160495662, -0.03916392089304226, -0.003697344732763326]
effort: []
time_from_start: 
  secs: 3
  nsecs: 876113560, positions: [0.15626173501370372, 1.2284994344959737, 3.141592653589793, -1.7815380224270412, 0.15759843202765386, 1.4408524005244572, 1.5502156823064446]
velocities: [0.024837842440575048, 0.19463696038281111, 0.0, 0.22282312240768615, 0.025623747066396075, -0.02732345908402934, -0.0077126574980013385]
accelerations: [-0.008716808476241317, -0.04343972160714914, 0.0, 0.0038494584977828276, -0.007515473760735204, -0.046578236003574684, -0.007770111598688883]
effort: []
time_from_start: 
  secs: 3
  nsecs: 991717136, positions: [0.15913570704522245, 1.2511924225099134, 3.141592653589793, -1.7551889144457757, 0.16057353723084541, 1.4373031009428767, 1.5492509344586431]
velocities: [0.022607273467243047, 0.1798026847741565, 0.0, 0.21143827346246002, 0.023485057746657115, -0.03074061131779908, -0.008138807588567932]
accelerations: [-0.027200416488006515, -0.1943132252327204, 0.0, -0.1834701763459862, -0.026857790406757197, -0.010989524263624732, 0.0004407116185146171]
effort: []
time_from_start: 
  secs: 4
  nsecs: 109849111, positions: [0.16195498189609756, 1.2738030807491094, 3.141592653589793, -1.7282153548719705, 0.16351422510297492, 1.4330597557008715, 1.548156093655498]
velocities: [0.019029166358668574, 0.1537431916635954, 0.0, 0.18561434461560483, 0.019925551477964265, -0.031014992395196034, -0.007871852853079588]
accelerations: [-0.02508793571282256, -0.18595532626281666, 0.0, -0.19204286182666747, -0.02512955074380264, 0.005688321469457203, 0.0032299602180710088]
effort: []
time_from_start: 
  secs: 4
  nsecs: 244832667, positions: [0.16472106199074335, 1.296350759048232, 3.141592653589793, -1.700606503061012, 0.1664241972342211, 1.4281317315242579, 1.5469266175565075]
velocities: [0.015082222317349325, 0.12384902357850176, 0.0, 0.15332744882263913, 0.01593311353128775, -0.028711827640100437, -0.007092283601102361]
accelerations: [-0.022595180769892573, -0.17438529569158162, 0.0, -0.1953816297426152, -0.023053708960826234, 0.02034752226565686, 0.005843282508601463]
effort: []
time_from_start: 
  secs: 4
  nsecs: 405910958, positions: [0.16743539682484496, 1.3188553749658278, 3.141592653589793, -1.6723489886464702, 0.16930743980015941, 1.422526452819904, 1.5455578143734143]
velocities: [0.009663412787011489, 0.08058914739552878, 0.0, 0.10201425447806936, 0.010301665391018807, -0.020878024664385205, -0.005074820091695651]
accelerations: [-0.021153020732173948, -0.17239634536371876, 0.0, -0.21123048069568823, -0.02223473563942194, 0.037820571294886175, 0.009385532777852465]
effort: []
time_from_start: 
  secs: 4
  nsecs: 614831724, positions: [0.1700993851812349, 1.3413375046369664, 3.141592653589793, -1.6434266993831907, 0.1721682535590082, 1.416249287873988, 1.5440447630143295]
velocities: [0.01977415735409563, 0.02552302731568939, 0.0, 0.022842540812814168, 0.019914698459561783, 0.0031536432498902485, -0.0024432476709090904]
accelerations: [0.0628159477637661, -0.13057617994160017, 0.0, -0.21468175113497967, 0.06128528251568274, 0.08450554486113925, 0.005396626859071866]
effort: []
time_from_start: 
  secs: 5
  nsecs:  35373893, positions: [0.18455596207245176, 1.3402868675056425, 3.141592653589793, -1.6534763001054331, 0.18654346054816776, 1.425491444829706, 1.543483871162134]
velocities: [0.017058726978457724, -0.03562332710070508, -0.0006608312962669819, -0.03494601093027065, 0.017113148340862194, -0.00031323873966791586, -0.0021240234375207888]
accelerations: [-0.0973784658247273, -0.20017977109079582, -0.003983348463304619, -0.07147311150724299, -0.09592355826144662, -0.12987999125346933, -0.00503553217867558]
effort: []
time_from_start: 
  secs: 5
  nsecs: 470633837, positions: [0.1847623282128928, 1.3245700027028453, 3.1412908733257927, -1.664163073628612, 0.18681736515036526, 1.4205000386729245, 1.5428081376973017]
velocities: [0.0011117523387477004, -0.08466227534945617, -0.0016277726860890241, -0.05651489619549804, 0.0014922836797368602, -0.027919562579367202, -0.0038476982106325883]
accelerations: [0.002161970644333078, -0.16456348826371825, -0.0031823334209038915, -0.1009622810877795, 0.0030429633074313526, -0.06299415069919025, -0.009234677782700346]
effort: []
time_from_start: 
  secs: 5
  nsecs: 698967654, positions: [0.18496876831527534, 1.3088502856386888, 3.14098835991873, -1.6745227528707278, 0.18709658719838862, 1.4151847720468473, 1.5420672968919533]
velocities: [0.0009563043595518414, -0.0728131155391751, -0.0014023390410568886, -0.04751366632222175, 0.0013024330381198569, -0.025083265634624034, -0.003525442820716009]
accelerations: [-0.0028797663501320923, 0.2193338879945482, 0.004212119485498798, 0.14828639951950526, -0.0038239146510011387, 0.0704925802272745, 0.009592724654281608]
effort: []
time_from_start: 
  secs: 5
  nsecs: 855395649, positions: [0.18517530199680268, 1.2931277859855, 3.1406850204997423, -1.6845554513438596, 0.18738218983072635, 1.4095458844936102, 1.5412609072674872]
velocities: [-0.007928135432953955, -0.007966634347011343, -0.005013764240035792, -0.028407841702141244, -0.009899058127156071, 0.01898925396651288, 0.009917052240393793]
accelerations: [-0.049111550913813466, 0.21421967567999622, -0.023878288065867597, 0.002265008835681845, -0.06177933203710152, 0.2027438438839944, 0.07049970053822063]
effort: []
time_from_start: 
  secs: 6
  nsecs: 203743209, positions: [0.1794893706699524, 1.3032216899622133, 3.1375198414843792, -1.6942392575562883, 0.1802552327947283, 1.4282692885863701, 1.5489170834975274]
velocities: [-0.03056896366682492, 0.05498671193955633, -0.0172623613944594, -0.04980775568133626, -0.037700432750892535, 0.09946017953296792, 0.04010664260863186]
accelerations: [-0.1190698584783862, 0.21744442657540491, -0.06835338923796032, -0.18377584268137134, -0.14405330600677455, 0.3819577973704962, 0.15143372282379208]
effort: []
time_from_start: 
  secs: 6
  nsecs: 549410049, positions: [0.17373927938457093, 1.3136146245475175, 3.1342557488433673, -1.7034520954944714, 0.17320633501447041, 1.4468947891674107, 1.5563881994864777]
velocities: [-0.044559300728769624, 0.08119599658548762, -0.02552752800025905, -0.06918540497907329, -0.054035647954160534, 0.14310829511353007, 0.05687752433396641]
accelerations: [0.001994904306974951, 0.006530858173209856, -0.0024583894289236688, 0.03721756061452704, 0.011514250143426892, -0.025364215624727442, -0.01828719983630558]
effort: []
time_from_start: 
  secs: 6
  nsecs: 678079789, positions: [0.16792287019327665, 1.3242996566256684, 3.1308929978727837, -1.712193124687055, 0.16623022881756902, 1.4654139296906787, 1.5636788016790806]
velocities: [-0.031466124097764614, 0.05805521547603941, -0.018284778988187932, -0.04638314148517034, -0.03750416970276165, 0.09966310362612733, 0.03902971996070971]
accelerations: [0.11549807077284184, -0.20994491634066387, 0.06594944627948542, 0.18163468333168645, 0.14062719304314877, -0.37240272532951546, -0.14843882729846578]
effort: []
time_from_start: 
  secs: 6
  nsecs: 808992012, positions: [0.16203808413466483, 1.3352695730866337, 3.127431765388783, -1.7204613276447018, 0.1593220982156902, 1.4838178522786927, 1.5707932731134113]
velocities: [0.01039049092642624, 0.018066743999374885, 0.0068962114734903435, 0.006548864319851366, 0.012035067105337647, 0.012568076711860533, -0.005810415981780578]
accelerations: [0.19439393733977609, -0.1105012075898097, 0.11961666173155004, 0.21896555370270154, 0.22710645353590034, -0.3047544797718156, -0.18959135946208525]
effort: []
time_from_start: 
  secs: 7
  nsecs: 127047162, positions: [0.17289866387681962, 1.3357237751179067, 3.134253576835355, -1.7096531470923164, 0.17198155756645747, 1.4747696848714984, 1.561396280487451]
velocities: [0.04294368518234573, 0.0007213277699183409, 0.026566456112621623, 0.024736191860804627, 0.049692206112085135, -0.01879114708740959, -0.03412493853459303]
accelerations: [0.031249604274624938, -0.007867682940116175, 0.016148948819192585, -0.1225779154655751, 0.033314489630594406, 0.11898337605579454, -0.0011562629856119381]
effort: []
time_from_start: 
  secs: 7
  nsecs: 403514642, positions: [0.18184925130999663, 1.3356853213511015, 3.1397191141650937, -1.7076598860889598, 0.1822746919728608, 1.4738373452838067, 1.5548163605650598]
velocities: [0.03437148880930957, -0.0013313422530590745, 0.01718902606857817, -0.011959213060537426, 0.03832538615083526, 0.013937430810630393, -0.018086493417619743]
accelerations: [-0.09621851897389815, -0.008897187880821661, -0.08863964636756763, -0.1757045175360536, -0.12010295268411499, 0.14781394805612721, 0.12722074281451853]
effort: []
time_from_start: 
  secs: 7
  nsecs: 595570927, positions: [0.18885565367221388, 1.334906017399428, 3.141592653589793, -1.718513956955128, 0.18957143071093607, 1.4841953196610025, 1.5542110691768145]
velocities: [0.011838019469046994, -0.003796133409239899, 0.0025910271467156926, -0.0630976467196323, 0.011588202153811284, 0.05891468973219775, 0.007543924472298954]
accelerations: [-0.07347863605375803, -0.00951329826301755, -0.023746521894937887, -0.20544029286564594, -0.08180468115777384, 0.1867843429559904, 0.06745510947896682]
effort: []
time_from_start: 
  secs: 7
  nsecs: 912044271, positions: [0.18923113316037335, 1.3336528727477082, 3.1414123697023477, -1.7409635327493813, 0.1896007440959752, 1.504984149823134, 1.5583640623167947]
velocities: [0.009411894909276936, -0.005381168906942731, 0.0002471043381098512, -0.11206578242040889, 0.008179917765071125, 0.10491049278799286, 0.019436073639036832]
accelerations: [0.08064435953946446, -0.0025741674664127656, 0.010088203850745698, -0.20652961290365257, 0.08253960677344689, 0.20287414677570276, 0.024942466631833932]
effort: []
time_from_start: 
  secs: 8
  nsecs: 156331331, positions: [0.1917603489981317, 1.332828779354558, 3.141592653589793, -1.7603105126505436, 0.19197678734135964, 1.5232320170578835, 1.5615640980259151]
velocities: [0.019867304790309646, -0.005100180277314972, 0.0006161044144283842, -0.1446723586321753, 0.018789120162307514, 0.1372965092369229, 0.025164530107070725]
accelerations: [0.03850060665791623, 0.007942450863559165, -0.009191966437833375, -0.18558561976119647, 0.03803397324321339, 0.1876205400892208, 0.04912759640898486]
effort: []
time_from_start: 
  secs: 8
  nsecs: 302640855, positions: [0.19449441043266655, 1.3322724360344735, 3.141592653589793, -1.7794460758189987, 0.19457572073517845, 1.5414858429081664, 1.5650300946921678]
velocities: [0.024499393552625315, -0.00366019713454611, 0.0, -0.16793816917330623, 0.023602400836016384, 0.1613112496326436, 0.031098417268846233]
accelerations: [0.0360374799900016, 0.015943536681174805, 0.0, -0.19018281185883548, 0.03976973298585999, 0.20094270926885563, 0.046392883228872375]
effort: []
time_from_start: 
  secs: 8
  nsecs: 424436929, positions: [0.19730654698171352, 1.3319808982952528, 3.141592653589793, -1.7983799225999337, 0.19731535604093425, 1.5597826773712993, 1.5686036034681212]
velocities: [0.025705061383999986, -0.0013821015770962889, 0.0, -0.1696042650662347, 0.025358910742999785, 0.1650087851429976, 0.032723679484440805]
accelerations: [-0.01397884689048548, 0.02264848849998881, 0.0, 0.151388810410879, -0.008386847866373242, -0.127939848843321, -0.016786884881437352]
effort: []
time_from_start: 
  secs: 8
  nsecs: 530351752, positions: [0.20069053712630797, 1.3319793144281085, 3.141592653589793, -1.8202205251909358, 0.20069828749951413, 1.5811908643740225, 1.572919882085642]
velocities: [0.02310250965751366, 0.0011160591296655576, 0.0, -0.14601335090072745, 0.023386144964339467, 0.1441334676181995, 0.0295394892392455]
accelerations: [-0.02341358082847869, 0.015030196942935696, 0.0, 0.19233116844587175, -0.01952956060356061, -0.1750485088240314, -0.02890113419484858]
effort: []
time_from_start: 
  secs: 8
  nsecs: 666478034, positions: [0.2041909969722662, 1.3323472632111504, 3.141592653589793, -1.8417985682401983, 0.20429304832478076, 1.60267327126316, 1.5774084119918892]
velocities: [0.017642291907953384, 0.002727910073292987, 0.0, -0.113587281227913, 0.018351270002775935, 0.1138004576092049, 0.024072453587829883]
accelerations: [-0.041563176861981486, 0.005433487567219127, 0.0, 0.20195864591674417, -0.04006013337000184, -0.19302135940784268, -0.037019091158781076]
effort: []
time_from_start: 
  secs: 8
  nsecs: 830466091, positions: [0.20687331775895298, 1.332965380733836, 3.141592653589793, -1.8601938312856037, 0.20713757509263542, 1.6212628067244255, 1.5814060463392947]
velocities: [0.008419296275297066, 0.003911532066655872, 0.0, -0.094391007737504, 0.009446410215522314, 0.09613393090683225, 0.020568363780103543]
accelerations: [-0.05688875826784949, 0.007209342957916777, 0.0, 0.012373876212983519, -0.054990163674568326, -0.004814971985462705, -0.002117717483457412]
effort: []
time_from_start: 
  secs: 9
  nsecs:  22902176, positions: [0.2074406761016405, 1.333867532726514, 3.141592653589793, -1.8784267551661595, 0.20794193081606022, 1.6399802249873154, 1.5853900930217582]
velocities: [-0.00426617219508472, 0.0035504196864440908, -0.004483323892060118, -0.06390567381128395, -0.0044574146623392406, 0.06522441944689186, 0.017739458224134884]
accelerations: [-0.053311821480638295, -0.00789021973895617, -0.03335390332900819, 0.21786563609788015, -0.06374624885921919, -0.22647734554884025, -0.01951720930174642]
effort: []
time_from_start: 
  secs: 9
  nsecs: 218554416, positions: [0.2035307026958749, 1.3347190955708828, 3.1385259270444337, -1.8902676093557431, 0.2034868526393249, 1.65187620768686, 1.590559995403958]
velocities: [-0.02662556807758391, 0.0006918394276913997, -0.020196948831599913, 0.0010394051941637963, -0.03150999485240614, -0.0035848954479712203, 0.017007573318152905]
accelerations: [-0.09356838079953182, -0.011072982430514852, -0.06916171231328301, 0.21961355385041492, -0.1138336499120456, -0.2362826053868851, 0.01164912698332065]
effort: []
time_from_start: 
  secs: 9
  nsecs: 560569249, positions: [0.19067158607490314, 1.3343789561096753, 3.1288622084924658, -1.8789826425962572, 0.1881139502520137, 1.638976226141148, 1.5963713807613562]
velocities: [-0.03134955281576878, -0.0006365358499982875, -0.02348525846325216, 0.025920656257780353, -0.037410910168571995, -0.02957424390108674, 0.0146610456720864]
accelerations: [0.06739333914836357, 0.0030230687502085032, 0.051123931824310874, -0.06938644623440327, 0.08099952755984838, 0.07967677625269186, -0.02728125124689965]
effort: []
time_from_start: 
  secs: 9
  nsecs: 868064133, positions: [0.184117383897147, 1.3343265640674558, 3.123983232912785, -1.874229741631033, 0.18032059004244222, 1.6335782657823885, 1.5996431112469558]
velocities: [-0.009721261881146734, -0.03626952444093415, -0.00884554066276447, -0.016628662203444826, -0.012721258673193899, -0.0205528395078509, 0.003912883798221968]
accelerations: [0.06784588135147959, -0.21950350089093024, 0.04072210169644274, -0.19316297971900281, 0.07360769730989304, -0.020405714439552452, -0.039581207297919155]
effort: []
time_from_start: 
  secs: 10
  nsecs: 181960533, positions: [0.18461192474747967, 1.3094305808894626, 3.123244385781726, -1.8908789574488818, 0.18010913101336481, 1.6253535639458498, 1.5987496773055725]
velocities: [0.0013798048883228945, -0.06926686531590065, -0.0020713754428738533, -0.04525589769439137, -0.0006244978308831775, -0.02393034034159458, -0.0026864688129560627]
accelerations: [-0.0003200795233186641, 0.017191414819968633, 0.00042318048620470074, 0.01740054397812383, -5.4202335979778436e-05, -0.0001178747374375136, -0.000494236698412937]
effort: []
time_from_start: 
  secs: 10
  nsecs: 525960077, positions: [0.1851123214730402, 1.2843872255544693, 3.1224892662933446, -1.9068194299858265, 0.17986904142624846, 1.6162874392276798, 1.5976990087078835]
velocities: [-0.008167695866893282, -0.022661024039644442, -0.0065775473215377665, 0.0033987483569792067, -0.0106049733375913, -0.026485302066973866, 0.0011058721059179783]
accelerations: [-0.04497196553407868, 0.2061507812744167, -0.021717160413335553, 0.21568112580938878, -0.04725144213132227, -0.01200715554064848, 0.01839511406913728]
effort: []
time_from_start: 
  secs: 10
  nsecs: 904478114, positions: [0.17689218645294366, 1.2940887692623948, 3.117293818187608, -1.8840499354330522, 0.17029034508098967, 1.6027780817967527, 1.6000208600874974]
velocities: [-0.029486396404567804, 0.03539985381099989, -0.01826757192685859, 0.08329724743603723, -0.03390915504751378, -0.04931947921693366, 0.008536284563771451]
accelerations: [-0.07147810619725667, 0.08798225704031537, -0.042947317351761806, 0.2077865800635702, -0.08056909652080115, -0.12266801015543004, 0.02144399501323373]
effort: []
time_from_start: 
  secs: 11
  nsecs: 370013466, positions: [0.16877658132968046, 1.3039024671975579, 3.1123093982060173, -1.860933184394987, 0.16101044954799484, 1.589102596291569, 1.60239473831384]
velocities: [-0.046535895367787336, 0.05704297113327754, -0.028124069955478517, 0.13460806853774196, -0.05266154481622103, -0.0795034345104313, 0.013870493280913804]
accelerations: [-0.05944074075450815, 0.08064525745232716, -0.031301238731160336, 0.19269676075033287, -0.06169984472510561, -0.11253354098409896, 0.020328796277571403]
effort: []
time_from_start: 
  secs: 11
  nsecs: 566443920, positions: [0.16076074492024348, 1.3138340302313736, 3.1075278783529323, -1.837464471044074, 0.15201516627355013, 1.5752586866887928, 1.6048194668081972]
velocities: [-0.046459449378103285, 0.05819064087491821, -0.027354201532847428, 0.13767673013888038, -0.05170984424209697, -0.08111095602089921, 0.014257875029582817]
accelerations: [0.0610066898825976, -0.0683571524168948, 0.04052907473865945, -0.1595761029449079, 0.0733724765921698, 0.09531690089575484, -0.01610161692559072]
effort: []
time_from_start: 
  secs: 11
  nsecs: 721320219, positions: [0.1528400369345103, 1.3238893085237626, 3.102941374733087, -1.8136380937068535, 0.14329074986800056, 1.5612433883406676, 1.6072940258089818]
velocities: [-0.03148664740853356, 0.04031293819797348, -0.01804448773270626, 0.0956024327993824, -0.0344618428915858, -0.05618110469337191, 0.009944589545393536]
accelerations: [0.07018786314484915, -0.08663024643680478, 0.04200563354963298, -0.2046982476648716, 0.07890368550675876, 0.12080599171097095, -0.02114699364568931]
effort: []
time_from_start: 
  secs: 11
  nsecs: 913745464, positions: [0.14500992816543726, 1.3340743247655504, 3.098542264111328, -1.789447330469121, 0.13482389374702092, 1.5470530571092727, 1.6098175695012689]
velocities: [0.00937348219054738, 0.013530643607522042, 0.007520191435813449, 0.043295388168149984, 0.011765134333914134, -0.02875488902982569, -0.011664540971999735]
accelerations: [0.20034042133647265, -0.09533648242247461, 0.1270357361885818, -0.15475118975483013, 0.22710009210496165, 0.06920560352232344, -0.12009709315223839]
effort: []
time_from_start: 
  secs: 12
  nsecs: 272747326, positions: [0.15570190150239857, 1.3337292193347556, 3.10573763286683, -1.78438381255594, 0.14724443706726997, 1.5423124216151047, 1.6018143777268183]
velocities: [0.047781005397471495, -0.0012050390677709152, 0.032169924186991786, 0.03173047274717866, 0.055586057231785405, -0.029408759796601958, -0.03741648670719686]
accelerations: [0.06800379776860314, 0.0009795404392590917, 0.04590360053533128, 0.11789923898567098, 0.07975397701336595, -0.1075721672831736, -0.06644856519064644]
effort: []
time_from_start: 
  secs: 12
  nsecs: 536370830, positions: [0.1645713830422414, 1.3335516832477459, 3.1117112870734864, -1.7772478913938554, 0.15757374431891352, 1.5357277587684475, 1.594642820635477]
velocities: [0.0521740659889495, -0.0012635954448052063, 0.03482832698628847, 0.026155282157109667, 0.060610558576945556, -0.02301205497869951, -0.03964883261305993]
accelerations: [-0.03229479686306429, -0.001855409692553547, -0.025302356555489656, -0.20651512145420722, -0.03933120196899419, 0.20337245476498927, 0.05506519797096423]
effort: []
time_from_start: 
  secs: 12
  nsecs: 697621607, positions: [0.17391195610979537, 1.3332817104667136, 3.1178843880737244, -1.775722736608581, 0.16839457943500183, 1.5347454685204864, 1.5880509615471994]
velocities: [0.037919764966930336, -0.0014006743980352772, 0.02485853952461815, -0.00925582592448504, 0.04378759878954726, 0.011041478427761448, -0.023941780830241313]
accelerations: [-0.10711075816614665, 0.00023929836927299197, -0.0726852130581508, -0.1623227184870108, -0.12541255862376294, 0.15217651436998803, 0.10202182694599396]
effort: []
time_from_start: 
  secs: 12
  nsecs: 886917155, positions: [0.18020022181991924, 1.3329553432249177, 3.121944225283406, -1.7820283240472787, 0.1756121942342424, 1.5412180166207117, 1.5849512999367412]
velocities: [0.004000917329217112, -0.0008095916724329558, 0.0016454059560133544, -0.05413754320302653, 0.004181466268679202, 0.05277861785637476, 0.00725604460066661]
accelerations: [-0.1675388450679448, 0.004212238233005489, -0.11515030429470544, -0.20533054467764483, -0.19535881181483852, 0.1899698619777054, 0.15131541812568414]
effort: []
time_from_start: 
  secs: 13
  nsecs: 124248984, positions: [0.1746570326543742, 1.332882199164016, 3.1178033258420617, -1.8065183343412399, 0.1690035415861725, 1.5646825424238122, 1.5932156662990828]
velocities: [-0.019582672557427103, 0.0006187977864800894, -0.014970545319848955, -0.1037462464226297, -0.023243142801487548, 0.10084399492117987, 0.03206949868613147]
accelerations: [-0.009834362400596618, 0.007793060918317862, -0.010433683726954628, -0.19906321137354716, -0.010789463598450877, 0.20375161917038787, 0.0406164144527549]
effort: []
time_from_start: 
  secs: 13
  nsecs: 423980716, positions: [0.17169813448514307, 1.3330942777368382, 3.1154951046495705, -1.8245232184532143, 0.16550555111400597, 1.5823463032530236, 1.5984497498686723]
velocities: [-0.025947273868805496, 0.002679945923851471, -0.020006908558108646, -0.1305662932895272, -0.03041747227991668, 0.12898665914446683, 0.04068366520433808]
accelerations: [-0.07857429308086439, 0.017847091561963003, -0.05780399160566095, -0.0711941209463407, -0.08905900383860148, 0.08316141727113852, 0.06131946525956989]
effort: []
time_from_start: 
  secs: 13
  nsecs: 567119661, positions: [0.16778165424678665, 1.3335807501484414, 3.1124986893269675, -1.8415004901834844, 0.16094004453665872, 1.5992262236197936, 1.6040693744603927]
velocities: [-0.033053476746626585, 0.0044737436956198895, -0.02520255007704751, -0.12183781550058277, -0.03842724953043926, 0.1219030680556081, 0.04330623296428347]
accelerations: [-0.026803203662799396, 0.008719628163538438, -0.019247863765160966, 0.19780864110262728, -0.029725849712639543, -0.185488584762749, -0.021886138980620966]
effort: []
time_from_start: 
  secs: 13
  nsecs: 692555255, positions: [0.16262819611110113, 1.3343296347591576, 3.1085812862404834, -1.857504126672072, 0.1549632239627688, 1.615363795310026, 1.6102462690861867]
velocities: [-0.036701658215813907, 0.005299594318065934, -0.027892919905037103, -0.09539129505593022, -0.04258022908247571, 0.09680975953548013, 0.040450971817718756]
accelerations: [-0.02343004638577337, 0.0029694054577226207, -0.01773483779159937, 0.16676374666303143, -0.027364033411913707, -0.1601661447964082, -0.017537867729030366]
effort: []
time_from_start: 
  secs: 13
  nsecs: 840286895, positions: [0.1563651320745851, 1.3352287798473277, 3.103822321114697, -1.870910655519882, 0.1476947084998522, 1.62908418423424, 1.6166021674602946]
velocities: [-0.026878230055259213, 0.003924957805975301, -0.020464798904131287, -0.05515121683824231, -0.03119476057262153, 0.05668605699403194, 0.02676168239226359]
accelerations: [0.08635499962337248, -0.011906059021453577, 0.0653088192601984, 0.20253009210217998, 0.10020635579927703, -0.2054637699653159, -0.09145493592059656]
effort: []
time_from_start: 
  secs: 14
  nsecs:   2881950, positions: [0.1506264155657219, 1.3361025399280106, 3.099430534953246, -1.8813993736910495, 0.14103352060781213, 1.6400020193382452, 1.622038055689417]
velocities: [-0.006603485636780448, -0.035666929417026205, -0.007493267799734632, -0.03858183448448071, -0.009883044547503127, 0.002274110524765312, 0.006654250370841839]
accelerations: [0.048306360234321666, -0.21254324865460011, 0.023317958541625366, -0.06005237448664709, 0.043660831425230015, -0.14947064067299487, -0.043523524324456206]
effort: []
time_from_start: 
  secs: 14
  nsecs: 379510816, positions: [0.1513131392013641, 1.3111874617483394, 3.0983055222121743, -1.8980812007365808, 0.14032999965006338, 1.6317346019527192, 1.621657664788795]
velocities: [0.001866937922555074, -0.06750368162948937, -0.0030714964455756623, -0.04420475258108685, -0.001959090855932687, -0.023378273709014158, -0.0011850857793449829]
accelerations: [-0.0008737970419907148, 0.032938688293158845, 0.0013617571434189223, 0.027367827351913076, 0.000646193866427839, 0.00568685313896492, -0.00032443358922920883]
effort: []
time_from_start: 
  secs: 14
  nsecs: 717783590, positions: [0.15200928082051698, 1.2861193330762895, 3.097154442133608, -1.9140547350848276, 0.1395788422040399, 1.6226164374971883, 1.6211487061719168]
velocities: [0.012054591767833248, -0.025323210947602592, 0.007225553796958237, 0.007954052885915212, 0.012817664317833372, -0.032406095465712215, -0.011393975481723023]
accelerations: [0.04755927160417267, 0.16555012003933636, 0.04614402806637508, 0.21617725631745965, 0.06734099969854145, -0.0463592847047663, -0.04662887757863041]
effort: []
time_from_start: 
  secs: 15
  nsecs: 126368372, positions: [0.16236002697660326, 1.2910657670506858, 3.105131995778963, -1.8886447417850225, 0.15227105146225392, 1.6029844453199107, 1.6111966945363076]
velocities: [0.03375217284920155, 0.017210365713341808, 0.025668423083754374, 0.08540595158728678, 0.04124243371849101, -0.06497605155568258, -0.03346939654248853]
accelerations: [0.0661368502435274, 0.03790533279902642, 0.04896145916940923, 0.17721003024675264, 0.08025330434218596, -0.13103081100183464, -0.0695194201783477]
effort: []
time_from_start: 
  secs: 15
  nsecs: 588343965, positions: [0.17247504006569492, 1.2963843690092136, 3.1127730460574905, -1.8626705299183195, 0.16460921051015884, 1.5833693030455345, 1.6010149444196633]
velocities: [0.0516123832736094, 0.028591208797632963, 0.03855419092963982, 0.13591068254939725, 0.06279959220201936, -0.10134964457105355, -0.05329756599854171]
accelerations: [0.0660570289712687, 0.04946706221864759, 0.045492333709814085, 0.2038643061037705, 0.07898822233644699, -0.14090087981732297, -0.0801298802931501]
effort: []
time_from_start: 
  secs: 15
  nsecs: 812628858, positions: [0.18236386093086426, 1.3020783475472941, 3.1200953287632216, -1.8361284821001385, 0.1766182099944489, 1.5637632715546599, 1.590603352839127]
velocities: [0.062379987135411735, 0.037653258666405634, 0.04570369143319012, 0.17141014581790254, 0.07561160547477894, -0.12515676458662742, -0.06725928140228321]
accelerations: [0.05397318050677608, 0.053088502640149894, 0.03379610230977982, 0.19535368990869123, 0.06373405051002047, -0.12577815857003513, -0.07689225928463908]
effort: []
time_from_start: 
  secs: 15
  nsecs: 982756718, positions: [0.19203597171685943, 1.3081511917308035, 3.1271159900000907, -1.8090128532774583, 0.18832259591762815, 1.5441574426103286, 1.5799607850659156]
velocities: [0.06362935655722109, 0.04159372302459891, 0.04575540652797494, 0.18210770895087153, 0.07690431743984329, -0.13035707929528811, -0.07149120944363137]
accelerations: [-0.03989460176243464, -0.0032392509438208274, -0.034680682012915104, -0.06240473194759574, -0.049533802810574463, 0.06258131214562128, 0.02427990687464494]
effort: []
time_from_start: 
  secs: 16
  nsecs: 127909162, positions: [0.2015007668887845, 1.3146067753809296, 3.1338515873076673, -1.781315735623012, 0.19974655183627468, 1.5245417860533639, 1.5690849752327267]
velocities: [0.05359593706306992, 0.037867840356539464, 0.03781947715600059, 0.15978216721572203, 0.06464594954496908, -0.11216076914868534, -0.06275264000081018]
accelerations: [-0.07916926747664906, -0.03921966094731967, -0.05996682765952447, -0.19852877337048236, -0.09605307665790086, 0.15186794502550002, 0.07783140476260772]
effort: []
time_from_start: 
  secs: 16
  nsecs: 284030639, positions: [0.21076752661093032, 1.321449475189478, 3.140318125130885, -1.7530269385541606, 0.21091394727252308, 1.5049051558969997, 1.55797242262212]
velocities: [0.053133851401059584, 0.03467431090194818, 0.01926827515019381, 0.1398587160135823, 0.059582361479396634, -0.09830840407648661, -0.04422354545191909]
accelerations: [0.06406685377941988, 0.002813578386939826, -0.12905377935579565, -0.022421959340434307, 0.033796464409384816, 0.0036058753370303647, 0.11336405900504987]
effort: []
time_from_start: 
  secs: 16
  nsecs: 483027705, positions: [0.22336329217558143, 1.3288260077496468, 3.141592653589793, -1.7240039968392498, 0.22421565054730488, 1.4842417309388043, 1.5510934710766542]
velocities: [0.045139215203905, 0.02659880747000761, 0.003020453128244873, 0.10354488105625398, 0.047454553520664244, -0.07358776037712988, -0.023691077725456186]
accelerations: [-0.09131826422226554, -0.052452869591481384, -0.01894233436505962, -0.21332451076947678, -0.09778157169667108, 0.1527141428100255, 0.055897949184426546]
effort: []
time_from_start: 
  secs: 16
  nsecs: 694010694, positions: [0.2364151545819668, 1.336609361507864, 3.141592653589793, -1.6943263202746695, 0.23781588436198015, 1.4632256513993807, 1.5447857231576736]
velocities: [0.016525055040856988, 0.008015124398308624, 0.0, 0.019757005933211225, 0.016837333338413914, -0.011071428482153896, -0.004213701540659859]
accelerations: [-0.0616100251311544, -0.04480485721419128, 0.0, -0.21820730063473567, -0.06587337426003968, 0.16732120656529378, 0.04631456187482646]
effort: []
time_from_start: 
  secs: 17
  nsecs: 120848381, positions: [0.2376154547018983, 1.335538889827078, 3.141592653589793, -1.7089000586915455, 0.23869563986388018, 1.476380950922236, 1.547869160612669]
velocities: [-0.02157118786757091, -0.0037592618375515727, -0.015109382010044437, -0.06455974401512973, -0.02641287644627453, 0.055311699623720435, 0.030631078502345914]
accelerations: [-0.14179756061757337, -0.009168321689789243, -0.08910914010882254, -0.2037301719556938, -0.16645847487199766, 0.16641771666166982, 0.14319746756504495]
effort: []
time_from_start: 
  secs: 17
  nsecs: 606394085, positions: [0.2288257381493859, 1.3345149333356994, 3.1357696209919177, -1.7279970560104856, 0.22816718884618248, 1.4924767556025909, 1.5584504211433123]
velocities: [-0.039635960699583486, -0.004628695254230773, -0.026602959853962868, -0.09525488624452416, -0.04758292048327582, 0.0817264119463057, 0.049508558785028585]
accelerations: [0.06580591498241645, 0.0075415474207433985, 0.03979963711819924, 0.042371171455553074, 0.07765226957307053, -0.019847438499994024, -0.05947308587278045]
effort: []
time_from_start: 
  secs: 17
  nsecs: 799090007, positions: [0.22308024466913912, 1.3338417514115986, 3.1318456032954614, -1.7436004059215484, 0.22124883114149457, 1.5061200407748798, 1.565979427522516]
velocities: [-0.023434027144336594, -0.0025130508581141333, -0.016038127687785807, -0.06251294372801469, -0.028163106148237874, 0.05486579574725796, 0.030343693172835088]
accelerations: [0.07415408978120724, 0.010375841331991337, 0.050403594213041206, 0.20956694248212765, 0.08968809245432109, -0.18175012299779178, -0.09981833540214599]
effort: []
time_from_start: 
  secs: 17
  nsecs: 969794802, positions: [0.21805010724039658, 1.333429550322374, 3.1283847828328675, -1.7564019236932797, 0.21523348380664878, 1.5174700377349408, 1.572293277781291]
velocities: [-0.005945123345647283, -0.03825617382857543, -0.005522698532347029, -0.04201873650777259, -0.007821446892480103, 0.0025051980379291287, 0.00642098472301886]
accelerations: [0.041453108969373695, -0.21209451538272445, 0.020348144660715974, -0.047916640251639685, 0.04551040192369399, -0.15577830705100348, -0.05797361263701669]
effort: []
time_from_start: 
  secs: 18
  nsecs: 350560838, positions: [0.21847302140373304, 1.3092686044942872, 3.1277581604495186, -1.772551026685046, 0.21528317055925172, 1.509527004028714, 1.5710953173481554]
velocities: [0.001140410463864758, -0.06513682046960648, -0.0016957831249130514, -0.042671541678712066, 0.0001300366544232377, -0.022257743067291, -0.0034252624523529135]
accelerations: [-0.0009443747403909136, 0.05402766838450637, 0.0013674360175308215, 0.040656040057579466, -0.00013166551200548363, 0.0133333924117873, 0.0016520462014490034]
effort: []
time_from_start: 
  secs: 18
  nsecs: 670871232, positions: [0.21889731170360113, 1.2850418075998788, 3.127124142833299, -1.7879793029411197, 0.21532953249432082, 1.5008169426652207, 1.569721269256266]
velocities: [-0.0010600580783822317, -0.008285886994192356, -0.0010203879771413964, -0.00912499720914517, -0.0014563377727300347, 0.000613771934785649, 0.0011987779804992855]
accelerations: [-0.009388476467634192, 0.21633105398886393, 0.0019277394101613402, 0.11988425513978054, -0.007254518465706581, 0.09446930171716163, 0.020023133712634904]
effort: []
time_from_start: 
  secs: 19
  nsecs: 112612586, positions: [0.21760615447259057, 1.301082543779496, 3.1268703604366346, -1.780989976204014, 0.21406477284984557, 1.5095955327894297, 1.5720298373219412]
velocities: [-0.005591479240476998, 0.06943156696304403, -0.0011150540371032578, 0.031322182089412745, -0.005384931674457325, 0.03696726936261087, 0.009707190332376108]
accelerations: [-0.017356883733995056, 0.21539626986319355, -0.003522346245580207, 0.10124437620555944, -0.016364463283150495, 0.11075645972681437, 0.02902718279758261]
effort: []
time_from_start: 
  secs: 19
  nsecs: 531736333, positions: [0.2163136583580732, 1.3171289489947253, 3.1266112021519934, -1.773657055302883, 0.21282812707160548, 1.5180484706364836, 1.574248186603802]
velocities: [-0.0060784342289350955, 0.07543989070366555, -0.0012265636491181797, 0.03500896965953663, -0.005772749452368167, 0.039224883266861924, 0.010289148727228676]
accelerations: [0.016912946220876142, -0.21017715541382456, 0.0033262217295497616, -0.09158242287549315, 0.016541684811910387, -0.11502426491390394, -0.030226917440714592]
effort: []
time_from_start: 
  secs: 19
  nsecs: 691257524, positions: [0.2150196788774201, 1.3331783361640375, 3.12634678225121, -1.765981742759137, 0.21161752648629786, 1.5261740040701994, 1.5763775207114366]
velocities: [0.011160173547600061, 0.024241677910253525, 0.00790597434060375, -0.0091446192453896, 0.013092493791467218, 0.03421638837350726, -0.0007935872021998611]
accelerations: [0.09945795965906597, -0.170268687917816, 0.05709714451618278, -0.21698965946722612, 0.1103816438916069, 0.05723806016990583, -0.04880229256492326]
effort: []
time_from_start: 
  secs: 20
  nsecs:  10402371, positions: [0.22274117746074973, 1.3326497970592548, 3.1312184427884526, -1.7783768546864924, 0.22039395048981594, 1.5387546045835145, 1.573959567538965]
velocities: [0.04372769450171573, -0.0024393370206727697, 0.02802443535243749, -0.06525218019236752, 0.04994258728361674, 0.06720090270319148, -0.014213546467747185]
accelerations: [0.16396920155729397, -0.005990455619916654, 0.10756857973152087, -0.21651106534525233, 0.18864570782892567, 0.2289379635541962, -0.056263675375111816]
effort: []
time_from_start: 
  secs: 20
  nsecs: 303162075, positions: [0.23071581739113994, 1.332248547922773, 3.136363581719375, -1.7898876950490887, 0.22952096075257555, 1.5506915725623833, 1.5713264548254144]
velocities: [0.055960550540306395, -0.002447792134474054, 0.036396270923744804, -0.0783761763356646, 0.06421275668965361, 0.08198823094811178, -0.018562596121391002]
accelerations: [-0.07131055532376339, 0.008712104900910688, -0.04195264347787225, 0.13634496563838702, -0.07930791528090342, -0.13149531764703945, 0.02235882111645921]
effort: []
time_from_start: 
  secs: 20
  nsecs: 433721537, positions: [0.23867914964423614, 1.3319631182574305, 3.141592653589793, -1.8006306858944776, 0.23868692571556993, 1.5620548708820203, 1.5686703797668031]
velocities: [0.061834288583071634, -0.0010031814882854864, 0.016692073064633984, -0.06300114569097062, 0.06565069963992212, 0.0640572101010813, -0.0016706190048805985]
accelerations: [0.13493174403616495, 0.010053222695514622, -0.20487086289829526, 0.06855639678716695, 0.0875362464735863, -0.10420104932796742, 0.18762138947881166]
effort: []
time_from_start: 
  secs: 20
  nsecs: 590354923, positions: [0.25100681017772974, 1.331931958125937, 3.141592653589793, -1.8103494490140006, 0.25100693725442547, 1.571460800754769, 1.5709751782081254]
velocities: [0.0848940897192946, 1.404569985725592e-05, 0.0, -0.055941218449351536, 0.08489288958756458, 0.05417392166046435, 0.013987702063334422]
accelerations: [0.15714120058502523, 0.002580321742383992, 0.0, 0.019199331514240334, 0.15771406946522723, -0.018146716801691616, 0.004840361516411613]
effort: []
time_from_start: 
  secs: 20
  nsecs: 759625822, positions: [0.26437463724851523, 1.3319612106427503, 3.141592653589793, -1.8178587727372009, 0.26438066342283634, 1.5787376118951124, 1.5729548936140219]
velocities: [0.08268248442128331, 0.0001797791956260455, 0.0, -0.03647765041260336, 0.08273181154288706, 0.03527241287054346, 0.010011076027479391]
accelerations: [-0.16648822185883227, -0.0003777676626584404, 0.0, 0.2097687132507943, -0.16641196663665375, -0.20415717669648203, -0.050704226285351046]
effort: []
time_from_start: 
  secs: 20
  nsecs: 897495067, positions: [0.2784087535206936, 1.331991448065724, 3.141592653589793, -1.8216518845633372, 0.27842624144566325, 1.5823821851791307, 1.5741166858574942]
velocities: [0.0710565242173655, -2.38602373422893e-05, 0.0, 0.003149468713016317, 0.07105859087092857, -0.0032050533993406016, -0.00035348334105484835]
accelerations: [0.02621872992251772, -0.0016931423329699714, 0.0, 0.21394080117191033, 0.025686789427999325, -0.20733189419889178, -0.059485086613778784]
effort: []
time_from_start: 
  secs: 21
  nsecs: 102657849, positions: [0.29310570004863623, 1.3319525457868178, 3.141592653589793, -1.816709470404085, 0.29311287268907016, 1.5775619661171632, 1.5728465993992344]
velocities: [0.0547802922254177, -0.00011626425231601138, 0.0, 0.017111554585122854, 0.054743534363221186, -0.016638687875715038, -0.004487463720963369]
accelerations: [-0.1382406942603905, 0.0005758004062515877, 0.0, -0.056059485093682464, -0.13813132086394578, 0.05503723215266584, 0.013747299559666805]
effort: []
time_from_start: 
  secs: 21
  nsecs: 302051324, positions: [0.30559263678795506, 1.3319395109533625, 3.141592653589793, -1.8134230699992167, 0.30559222287309196, 1.5743915106201756, 1.5719392469060003]
velocities: [0.018440291272511258, -0.037195260075755125, -0.0006908145536414261, -0.020086828204269446, 0.018250144143904273, -0.016549954664490593, -0.004389857589350213]
accelerations: [-0.10271085564480763, -0.2191882702842006, -0.004075007291673755, -0.1741497797191068, -0.10370401283259863, -0.04392860930154408, -0.010527545911338986]
effort: []
time_from_start: 
  secs: 21
  nsecs: 650339409, positions: [0.3059317681603685, 1.3074170516205978, 3.141136977283031, -1.8297848307265743, 0.3058131131403122, 1.5664770585577774, 1.569902817420798]
velocities: [0.0008673634412053757, -0.06274913837250054, -0.0011695087952176934, -0.04105388882638371, 0.000565076565107241, -0.02102562112116705, -0.00546235441714393]
accelerations: [-0.0007932827094505464, 0.057211981101515805, 0.0010458349657984126, 0.0421825838831356, -0.0005160719326512877, 0.014649752016422716, 0.003511335249044503]
effort: []
time_from_start: 
  secs: 21
  nsecs: 980150286, positions: [0.3062719210736005, 1.2827914306162527, 3.140676009406791, -1.8454322414051707, 0.3060347910168819, 1.5577841891956714, 1.5676156825483942]
velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
accelerations: [-0.0029345348076730195, 0.21244781151858283, 0.00397681814685063, 0.13499185072383557, -0.001912438258146556, 0.0749942944167985, 0.019731352082625355]
effort: []
time_from_start: 
  secs: 22
  nsecs: 461634954]

Furthermore, I found that FollowJointTrajectoryAction seems to be usable, but it cannot receive many points, not even 500. I also tried using the MATLAB example 'Send Precomputed Trajectory to the Robot', but an error message appeared when sending the trajectory to FollowJointTrajectoryAction. However, when I sent a trajectory with only a few points to FollowJointTrajectoryAction, it worked. This contradicts the description that PreComputedJointTrajectory can receive up to 30,000 points.

[ERROR] [1680496495.286302327]: Runtime exception detected while sending the trajectory
[ERROR] [1680496495.286365021]: timeout detected: BaseClient::ValidateWaypointList

[ERROR] [1680496500.331258774]: Runtime exception detected while sending the trajectory
[ERROR] [1680496500.331306895]: timeout detected: BaseClient::ExecuteWaypointTrajectory

[ERROR] [1680496500.331338189]: To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: 4

This is the environment I am using. OS: Ubuntu 20.04 ROS: noetic firmware: 2.3.0 ros_kortex: noetic-devel

ch1hha0 commented 1 year ago

Additionally, I also set the api_rpc_timeout_ms to 30000 based on the suggestion in #278 , but the error still persists.

felixmaisonneuve commented 1 year ago

Hi @ch1hha0,

Have you tried using PreComputedJointTrajectory with a smaller amount of points? Does the PreComputedTrajectory you send seem correct? (your #print(req.input.trajectory_elements) line in you original message)

If you are using the matlab API, I would suggest to look at our github repo rather than using the MatLab documentation. However, our MatLab API is still at Kortex2.2.0, so waypoints are not available on it, but the functions you mentionned so far should be.

I am sure I fully understand what you are doing to get the error message. Are you using MatLab or FollowJointTrajectoryAction?

Best, Felix

ch1hha0 commented 1 year ago

Sorry, I haven't been using this arm recently, so I haven't been able to respond. But what I can confirm is that I have tried using a PreComputedJointTrajectory with a few points before, but the arm still couldn't move. And, the first point of the trajectory I used is indeed the starting point of the arm. There were no error messages either. Additionally, could you provide an example code for a runnable PreComputedJointTrajectory? I believe this might help me resolve the issue faster. Thank you very much.

felixmaisonneuve commented 1 year ago

I looked into this a bit and played with PreComputedJointTrajectory.

Turns out your trajectory could fail with an event ACTION_PREPROCESS_ABORT, but it won't print because wait_for_action_end_or_abort only prints ACTION_ABORT and ACTION_END in our examples https://github.com/Kinovarobotics/ros_kortex/blob/373cacd4e3f7bba6dddaeb3451f31dbcaa64beb8/kortex_examples/src/full_arm/example_waypoint_action_client.py#L70-L79

This might explain why your arm does not move (because the Trajectory is invalid and the call fails) but nothing is printed in your terminal.

If that's the case, you can use the Base.GetTrajectoryErrorReport() API call (you can create a ServiceClient for base/get_trajectory_error_report) after the action has aborted to get more information about the problem in your trajectory

I did not test specifically for your test trajectory.

I will try to add an example for PreComputedJointTrajectory, probably in our Kortex repo.

In the meantime, you can try this and see what the problem is. If you have more questions, feel free to ask

Best, Felix

ch1hha0 commented 1 year ago

Thank you, Felix. I haven't been using this robotic arm recently, but I will continue to provide feedback if there are any new issues. I'm also looking forward to your adding an example for the PreComputedJointTrajectory.

ch1hha0 commented 1 year ago

Hello, I've been recently attempting to use follow_joint_trajectory to control a robotic arm. I've generated a trajectory that lasts for 4 seconds, with a time interval of 1 millisecond for each point. However, when I send this trajectory to follow_joint_trajectory, I encountered the following error:

[ INFO] [1691740327.414840312]: New goal received.
[ INFO] [1691740327.414875794]: Joint Trajectory Goal is accepted.
[ERROR] [1691740328.061721589]: Joint Trajectory failed validation in the arm.
[ERROR] [1691740328.061756755]: Error 1 : Waypoint list size should be between 2 and 1000. Interrupting validation

When I use MoveIt's compute_cartesian_path to generate a trajectory that lasts for 14 seconds and use execute to execute it, the following error occurs:

[ERROR] [1691729920.570040165]: Runtime exception detected while sending the trajectory
[ERROR] [1691729920.570139730]: timeout detected: BaseClient::ValidateWaypointList

[ INFO] [1691729920.570727564]: Controller 'gen3_joint_trajectory_controller' successfully finished
[ WARN] [1691729920.570940032]: Controller handle gen3_joint_trajectory_controller reports status ABORTED
[ INFO] [1691729920.571030258]: Completed trajectory execution with status ABORTED ...
[ INFO] [1691729920.571286200]: Execution completed: ABORTED
[ERROR] [1691729923.570473039]: Runtime exception detected while sending the trajectory
[ERROR] [1691729923.570524820]: timeout detected: BaseClient::ClearFaults

[ERROR] [1691729923.570565594]: To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: 4

I'd like to understand the correct way to use follow_joint_trajectory. The code I'm using is from Trajectory control, available at: https://github.com/akihikoy/ay_test/blob/master/ros/py_ros/gen3/follow_q_traj3.py When using this code, I made necessary modifications to support Python 3, but I'm still encountering the aforementioned error.

As far as I know, in previous versions, it was possible to use follow_joint_trajectory without the limitation of 2 to 1000 points. In the current latest noetic-devel version, is it possible to remove this limitation by making some code modifications?

felixmaisonneuve commented 1 year ago

Hi @ch1hha0,

Yes, we changed the joint action server with the Kortex2.3 release. It was previously pre_computed_joint_trajectory_action_server and it was using the PreComputedJointTrajectory call. It was changed to joint_trajectory_action_server so it could use the new Waypoint system (added in Kortex2.3).

You could use the old file that you could get from a commit older than #154

This was never used on the noetic branch however. I do not know how much work this would imply. I remember going over the same topic in another issue, but I can't find it.

Best, Felix

ch1hha0 commented 1 year ago

Hello, I've tried using PreComputedJointTrajectory again, and I used the trajectory file "trajectory.mat" provided by matlab_kortex. Here's the code I used to read the .mat file and input it into PreComputedJointTrajectory:

def load_trajectory():
    mat = loadmat("trajectory.mat")
    pre_compt_traj = mat['precomputedTrajectory']
    times = pre_compt_traj[0:2,0].astype('float32')
    positions = np.rad2deg(pre_compt_traj[:,1:1 + 7]).astype('float32')
    velocity = np.rad2deg(pre_compt_traj[:, 8:8 + 7]).astype('float32')
    accelerations = np.rad2deg(pre_compt_traj[:, 15 : 15 + 7]).astype('float32')
/
    goal = kortex_driver.msg.PreComputedJointTrajectory()
    goal.mode = kortex_driver.msg.TrajectoryContinuityMode.TRAJECTORY_CONTINUITY_MODE_ACCELERATION
    for i in range(times.shape[0]):
        point = kortex_driver.msg.PreComputedJointTrajectoryElement()
        point.joint_angles = [x for x in positions[i]]
        point.joint_speeds = [x for x in velocity[i]]
        point.joint_accelerations = [x for x in accelerations[i]]
        point.time_from_start = times[i]
        goal.trajectory_elements.append(point)
    req = kortex_driver.srv.ExecuteActionRequest()
    req.input.oneof_action_parameters.play_pre_computed_trajectory.append(goal)
execute_action_full_name = '/' + self.robot_name + '/base/execute_action'
rospy.wait_for_service(execute_action_full_name)
execute_action = rospy.ServiceProxy(execute_action_full_name, kortex_driver.srv.ExecuteAction)
#req.input.handle.action_type = kortex_driver.msg.ActionType.PLAY_PRE_COMPUTED_TRAJECTORY
execute_action(req)

I determined the meaning of each column of data in the trajectory.mat file by referring to the code in kObjTrajectoryFeeder.m from matlab_kortex_api. Hence, I'm confident that I didn't make any mistakes when passing the trajectory points to PreComputedJointTrajectory. Here is a portion of the code from kObjTrajectoryFeeder.m:

if coder.target('MATLAB')
                load('trajectory.mat');
            else
                traj = coder.load('trajectory.mat');
                precomputedTrajectory = traj.precomputedTrajectory;
            end

case enumCmd.precomputed_joint_trj
                            obj.currentPrecompIndex = obj.currentPrecompIndex + 1;
                            precompute_trj_timestamp = obj.trjPreCompWayPoints{obj.currentPrecompIndex}.wayPoint(:, 1).';
                            % 7 is always used for trajectories since output is fixed in size
                            % In the case of having less than 7 joints, the values for non-existant joints wil be left to zero.
                            precompute_trj_position(1:obj.nbrJointActuators, :) =     rad2deg(obj.trjPreCompWayPoints{obj.currentPrecompIndex}.wayPoint(:, 2:2+obj.nbrJointActuators-1).');
                            precompute_trj_velocity(1:obj.nbrJointActuators, :) =     rad2deg(obj.trjPreCompWayPoints{obj.currentPrecompIndex}.wayPoint(:, 9:9+obj.nbrJointActuators-1).');
                            precompute_trj_acceleration(1:obj.nbrJointActuators, :) = rad2deg(obj.trjPreCompWayPoints{obj.currentPrecompIndex}.wayPoint(:, 16:16+obj.nbrJointActuators-1).');
                            precompute_count = obj.trjPreCompWayPoints{obj.currentPrecompIndex}.count;
                            cmdToProcess = enumCmd.precomputed_joint_trj;

There are a total of 14601 trajectory points, which doesn't exceed the hard limit of PreComputedJointTrajectory. Below is a portion of the trajectory points printed using print(req):

input: 
  handle: 
    identifier: 0
    action_type: 0
    permission: 0
  name: ''
  application_data: ''
  oneof_action_parameters: 
    send_twist_command: []
    send_wrench_command: []
    send_joint_speeds: []
    reach_pose: []
    reach_joint_angles: []
    toggle_admittance_mode: []
    snapshot: []
    switch_control_mapping: []
    navigate_joints: []
    navigate_mappings: []
    change_twist: []
    change_joint_speeds: []
    change_wrench: []
    apply_emergency_stop: []
    clear_faults: []
    delay: []
    execute_action: []
    send_gripper_command: []
    send_gpio_command: []
    stop_action: []
    play_pre_computed_trajectory: 
      - 
        mode: 3
        trajectory_elements: 
          - 
            joint_angles: 
              - 0.0
              - 0.0
              - 0.0
              - 0.0
              - 0.0
              - 0.0
              - 0.0
            joint_speeds: 
              - 2.6896883e-05
              - -0.000680998
              - 6.2393665e-06
              - 5.8685768e-05
              - -1.6176098e-05
              - 0.0006113214
              - -1.6792254e-05
            joint_accelerations: 
              - -0.0025195824
              - -3.795089e-05
              - 0.0012612368
              - 1.3601614e-05
              - 0.0010790705
              - 3.0070032e-05
              - 0.00017750029
            time_from_start: 0.0
          - 
            joint_angles: 
              - 7.988643e-08
              - -6.768654e-07
              - 6.791547e-11
              - 5.4339388e-08
              - -4.804463e-08
              - 6.108203e-07
              - -2.5784434e-08
            joint_speeds: 
              - 2.4377301e-05
              - -0.000681036
              - 7.5006033e-06
              - 5.869937e-05
              - -1.5097028e-05
              - 0.0006113515
              - -1.6614753e-05
            joint_accelerations: 
              - -0.0025197407
              - -3.8571987e-05
              - 0.0012613377
              - 1.46927605e-05
              - 0.0010791013
              - 2.9769342e-05
              - 0.00017752695
            time_from_start: 0.001
          - 
            joint_angles: 
              - 1.581752e-07
              - -1.3538148e-06
              - 2.714388e-10
              - 1.0877258e-07
              - -9.51284e-08
              - 1.2216507e-06
              - -5.1539836e-08
            joint_speeds: 
              - 2.1857402e-05
              - -0.00068107515
              - 8.762042e-06
              - 5.871515e-05
              - -1.4017895e-05
              - 0.00061138096
              - -1.64372e-05
            joint_accelerations: 
              - -0.0025200574
              - -3.9809256e-05
              - 0.0012615394
              - 1.6864617e-05
              - 0.0010791629
              - 2.9171e-05
              - 0.00017758025
            time_from_start: 0.002
          - 
            joint_angles: 
              - 2.3488242e-07
              - -2.0308482e-06
              - 6.102353e-10
              - 1.6329939e-07
              - -1.4126101e-07
              - 1.8324912e-06
              - -7.726153e-08
            joint_speeds: 
              - 1.9337187e-05
              - -0.0006811156
              - 1.0023682e-05
              - 5.8733098e-05
              - -1.2938702e-05
              - 0.00061140984
              - -1.6259592e-05
            joint_accelerations: 
              - -0.0025203738
              - -4.1036663e-05
              - 0.001261741
              - 1.9015595e-05
              - 0.0010792245
              - 2.857873e-05
              - 0.00017763354
            time_from_start: 0.003
          - 
            joint_angles: 
              - 3.1002423e-07
              - -2.7079655e-06
              - 1.0839704e-09
              - 2.1791963e-07
              - -1.8645218e-07
              - 2.4433418e-06
              - -1.02944846e-07
            joint_speeds: 
              - 1.6816655e-05
              - -0.0006811572
              - 1.1285524e-05
              - 5.8753183e-05
              - -1.1859446e-05
              - 0.0006114381
              - -1.6081933e-05
            joint_accelerations: 
              - -0.0025206904
              - -4.2254207e-05
              - 0.0012619427
              - 2.1145695e-05
              - 0.0010792861
              - 2.7992537e-05
              - 0.0001776868
            time_from_start: 0.004
          - 
            joint_angles: 
              - 3.836168e-07
              - -3.3851668e-06
              - 1.6923093e-09
              - 2.7263314e-07
              - -2.307116e-07
              - 3.0542028e-06
              - -1.2858511e-07
            joint_speeds: 
              - 1.4295806e-05
              - -0.00068120006
              - 1.2547567e-05
              - 5.877539e-05
              - -1.078013e-05
              - 0.00061146583
              - -1.590422e-05
            joint_accelerations: 
              - -0.0025210069
              - -4.3461885e-05
              - 0.0012621442
              - 2.325492e-05
              - 0.0010793478
              - 2.7412418e-05
              - 0.00017774003
            time_from_start: 0.005
          - 
            joint_angles: 
              - 4.5567623e-07
              - -4.062452e-06
              - 2.4349174e-09
              - 3.274397e-07
              - -2.74049e-07
              - 3.6650736e-06
              - -1.5417767e-07
            joint_speeds: 
              - 1.1774641e-05
              - -0.0006812441
              - 1.3809812e-05
              - 5.8799695e-05
              - -9.700751e-06
              - 0.00061149296
              - -1.5726451e-05
            joint_accelerations: 
              - -0.0025213233
              - -4.4659704e-05
              - 0.0012623457
              - 2.5343266e-05
              - 0.0010794094
              - 2.6838372e-05
              - 0.00017779325
            time_from_start: 0.006
          - 
            joint_angles: 
              - 5.262187e-07
              - -4.739821e-06
              - 3.3114602e-09
              - 3.8233915e-07
              - -3.1647403e-07
              - 4.275955e-06
              - -1.7971782e-07
            joint_speeds: 
              - 9.25316e-06
              - -0.0006812894
              - 1.5072259e-05
              - 5.8826077e-05
              - -8.621311e-06
              - 0.0006115195
              - -1.5548632e-05
            joint_accelerations: 
              - -0.0025216395
              - -4.584766e-05
              - 0.0012625472
              - 2.7410735e-05
              - 0.001079471
              - 2.6270402e-05
              - 0.00017784644
            time_from_start: 0.007
          - 
            joint_angles: 
              - 5.952603e-07
              - -5.417274e-06
              - 4.3216026e-09
              - 4.3733132e-07
              - -3.5799647e-07
              - 4.886846e-06
              - -2.052009e-07
            joint_speeds: 
              - 6.731362e-06
              - -0.00068133586
              - 1.6334907e-05
              - 5.8854515e-05
              - -7.5418093e-06
              - 0.00061154546
              - -1.5370759e-05
            joint_accelerations: 
              - -0.0025219556
              - -4.7025755e-05
              - 0.0012627486
              - 2.945733e-05
              - 0.0010795326
              - 2.5708507e-05
              - 0.00017789961
            time_from_start: 0.008
          - 
            joint_angles: 
              - 6.6281723e-07
              - -6.094811e-06
              - 5.4650107e-09
              - 4.92416e-07
              - -3.9862599e-07
              - 5.497747e-06
              - -2.3062225e-07
            joint_speeds: 
              - 4.2092483e-06
              - -0.0006813835
              - 1.7597757e-05
              - 5.888499e-05
              - -6.462246e-06
              - 0.0006115709
              - -1.51928325e-05
            joint_accelerations: 
              - -0.002522272
              - -4.8193982e-05
              - 0.0012629499
              - 3.1483043e-05
              - 0.0010795941
              - 2.5152685e-05
              - 0.00017795275
            time_from_start: 0.009
    execute_sequence: []
    execute_waypoint_list: []

However, this still doesn't make the robotic arm move. The error message obtained using GetTrajectoryErrorReport is as follows:

output: 
  trajectory_error_elements: 
    - 
      error_type: 9
      error_identifier: 0
      error_value: 0.0
      min_value: 1.0
      max_value: 99999.9921875
      index: 0
      message: "Invalid Trajectory - Trajectory is empty"
      waypoint_index: 0

I've consulted TrajectoryErrorType.md, but I believe the trajectory should be valid since it's provided by matlab_kortex. I've also tried changing the goal.mode to other values and specifying req.input.handle.action_type = kortex_driver.msg.ActionType.PLAY_PRE_COMPUTED_TRAJECTORY. However, this still yields the same error message as mentioned above.

ch1hha0 commented 1 year ago

Hello, I have recently been using the Service method to use PreComputedJointTrajectory and have successfully moved the robot arm. However, I found that the description of the hard limits for precomputed_joint_trajectories in matlab_kortex (The acceleration limit of every joint must be respected for all the trajectory points. The trajectory will be rejected if any of the points contain an illegal acceleration. The hard acceleration limit for each joint is 1 degree / second ^ squared.) is incorrect. In the trajectory points provided in trajectory.mat, the maximum acceleration values for each joint are: 2.93391904269429 4.18121199820796 5.58093764832027 9.33660702926885 7.53745644870699 5.40291825949929 3.80256206887719 Therefore, I would like to know what the correct hard limits are when using PreComputedJointTrajectory.

shi00317 commented 4 months ago

Hi @ch1hha0, I have the same problem. I am wondering are you successful using play_pre_compute_trajectory with python or Matlab? Here is my code:

def play_pre_compute(traj):
    goal = PreComputedJointTrajectory()
    goal.mode = kortex_driver.msg.TrajectoryContinuityMode.TRAJECTORY_CONTINUITY_MODE_POSITION
    for i in traj:
        joint_list = PreComputedJointTrajectoryElement()
        joint_list.joint_angles = i['positions']
        joint_list.joint_speeds = i['velocities']
        joint_list.joint_accelerations = i['accelerations']
        joint_list.time_from_start = i['time_from_start']['secs']+i['time_from_start']['nsecs']
        goal.trajectory_elements.append(joint_list)

    service = rospy.ServiceProxy('/my_gen3/base/play_pre_computed_joint_trajectory', PlayPreComputedJointTrajectory)
    resp1 = service(goal)

The pre compute trajectory is made my moveit:

  positions: [-0.7116887886428893, 1.2937886912747407, -1.2954718090836703, -0.9481631123720398, 0.2918366823914488, -0.3021803667758034, 1.054209587767749]
  velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  accelerations: [9.237055564881302e-14, -2.418360650624507e-13, 2.3148150063434514e-13, -5.685035775471192e-14, -2.7200464103316335e-14, -3.147482274812319e-14, 2.6989521728637555e-13]
  effort: []
  time_from_start: 
    secs: 0
    nsecs:         0
- 
  positions: [-0.7115911248425385, 1.293793204077657, -1.2953108883778615, -0.9481476267502637, 0.29164175465029113, -0.30203616995857485, 1.054325526879847]
  velocities: [0.00864197345499454, 0.0003993242416344238, 0.014239385145404182, 0.001370275699313958, -0.01724856455181653, 0.012759538972555842, 0.010259100358032958]
  accelerations: [0.5098013452173966, 0.023556660596731924, 0.8399999999999508, 0.08083436016872421, -1.01751543873402, 0.7527019339320946, 0.6051977815578]
  effort: []
  time_from_start: 
    secs: 0
    nsecs:  33903298
- 
  positions: [-0.6859682906275401, 1.2949771719404148, -1.2530921281833007, -0.944084857146164, 0.24050099238579642, -0.2642050479982637, 1.0847430268915985]
  velocities: [0.15685905637290967, 0.007248069443441325, 0.25845676671773077, 0.02487165162964606, -0.3130758933102061, 0.2315963192217543, 0.18621126410260352]
  accelerations: [0.46170088359478606, 0.02133405710688794, 0.7607448388631067, 0.0732075265489838, -0.9215114505719482, 0.6816834660013223, 0.5480965342994647]
  effort: []
  time_from_start: 
    secs: 0
    nsecs: 339032980
- 
  positions: [-0.6602477926121908, 1.2961656526060892, -1.2107124472829311, -0.9400066019202882, 0.18916530238014406, -0.22622972922072404, 1.115276466015448]
  velocities: [0.1987437246048692, 0.009183456478019892, 0.327470161140694, 0.03151290589304014, -0.39667374367306796, 0.293437409042446, 0.23593358934373923]
  accelerations: [0.13012036652521616, 0.006012540648811779, 0.21439941048915984, 0.02063195138981042, -0.259707988366871, 0.19211767965366094, 0.15446910428015811]
  effort: []
  time_from_start: 
    secs: 0
    nsecs: 480577976
...

So I just wondering do you have any suggestion for my code.

Thanks.

ch1hha0 commented 4 months ago

Hi @shi00317 , Recently, I successfully used play_pre_compute_trajectory in Python, but the trajectory was derived from matlab_kortex. The trajectory I calculated myself still can not be executed. What I currently know is that the trajectory needs to be input at strict 1ms intervals. I suggest you refer to 01-actuator_low_level_velocity_control.cpp and use low_level to implement continuous trajectories.

martinleroux commented 4 months ago

Hi all,

The robot not moving when trying to launch a PreComputed trajectory indicates that at least one point of the trajectory is not within the limits of the arm.

As per our documentation in matlab_kortex, here are the validation that the robot does before launching the trajectory: Once sent to the arm, a trajectory is validated and the arm will execute it if it meets all the conditions :

The starting timestamp must be 0.0 seconds. Timestamps are all relative to the starting timestamp, which must be 0.0.

Time increments must be 0.001 seconds between trajectory points. For now, the arm only accepts trajectories with waypoints every 1ms.

The current position of the arm must match the first trajectory point. The trajectory planners communicating with the System object have to take into account the current position of the arm for the computation of the trajectories.

The position limit of every joint must be respected for all the trajectory points. The trajectory will be rejected if any of the points contain an illegal position.

The velocity limit of every joint must be respected for all the trajectory points. The trajectory will be rejected if any of the points contain an illegal velocity. The hard velocity limit for each joint is 50 degree / second..

The acceleration limit of every joint must be respected for all the trajectory points. The trajectory will be rejected if any of the points contain an illegal acceleration. The hard acceleration limit for each joint is 1 degree / second ^ squared.  ****This is a mistake, the actual limit is 1 rad/s^2, as indicated in our user guide***

The arrays must contain the same number of joints and number of trajectory points. The trajectory will be rejected if there is a mismatch in array sizes.

Position continuity must be ensured for all trajectory points. For now, position continuity is the only supported trajectory continuity mode.

The trajectory must not last more than 30 seconds. For now, it is the maximum supported size for a trajectory (30000 trajectory points).

To find out which point(s) triggered the rejection of your trajectory, as indicated above, you should refer to the GetTrajectoryErrorReport service (see here).

It has been known for a while that the trajectories generated by MoveIt can often exceed the robot's acceleration limits. The issue has been solved in our ROS2 implementation which uses our low-level interface instead of the PreComputedTrajectory endpoint. In these cases, MoveIt still exceeds the limits of the robot, but the robot provides a best effort until it ends up either lagging too far behind the commanded trajectory or, in most cases, it catches up.

As mentioned in @ch1hha0 's previous post, continuous trajectories can also be implemented directly using the low-level API outside of ROS.

Hope this helps.

*Edit: The maximum acceleration being 1 deg/s^2 is indeed a mistake. The actual value is 1 rad/s^2, as indicated in our User Guide.

aalmrad commented 1 month ago

Closing because of inactivity. Please reply to this thread to reopen.

Best, Abed

Kinovarobotics / ros_kortex

Drive gen3 using PreComputedJointTrajectory #286