[Baxter] Baxter's arm goes slightly down when trajectory starts

[pazeshun]

In https://github.com/RethinkRobotics/baxter_interface/pull/74/files#diff-393fafc5e38a00ca79cd9d500e6ab03cR367 , joint current positions are set to first trajectory point. And those positions are got from /robot/joint_states.

But when Baxter's hand grasps heavy object (or gripper is heavy), /robot/joint_states and commanded positions are very different. In that situation, Baxter's arm goes slightly down because first trajectory point is different from current commanded joint angles.

[pazeshun]

I don't want to revert #636 because potentio-vector sometimes returns old joint_states.
I sometimes want to add time_from_start = 0 point as first point.

[k-okada]

so can you set current reference position to the first trajectory?

…

-- ◉ Kei Okada 2016-12-05 14:25 GMT+09:00 pazeshun <notifications@github.com>:

I don't want to revert #636 <#636> because potentio-vector sometimes returns old joint_states. I sometimes want to add time_from_start = 0 point as first point. — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub <#738 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAeG3D_kNA9gB-ImuduEPJoSVPveJMUbks5rE6AugaJpZM4LD3L0> .

[pazeshun]

reference position

For now, we can't get reference position due to RethinkRobotics/baxter_interface#76
And even if issue above is fixed, I think setting reference position to first point is very dangerous because Baxter has Zero-G mode and we can move Baxter arm by hand.
Once Baxter arm is moved by hand, reference position which can be got from JTA and real position of joint are very different.
So I want to set first point whose time_from_start is 0 (or small enough) in angle-vector-sequence only when that point is necessary.

I'll send PR.

[pazeshun]

I understood what happens.

I want to set first point whose time_from_start is 0 (or small enough) in angle-vector-sequence only when that point is necessary.

In most cases, this can be realized by setting current desired angle vector to first point of avs because that angle vector and current joint_states are similar.

But in my new gripper(gripper-v5), joints sometimes move too slowly. Due to this, gripper doesn't complete its motion in time and joint_states and current desired angle vector are too different. In that situation, calculated time_from_start of first point is too big and arm goes down.

Also, when Baxter grasped dumbbell using RethinkRobotics/baxter_interface#75, same problem happened.

[pazeshun]

Anyway, I want to allow time_from_start of first point to be zero.

[pazeshun]

@k-okada Do you have any ideas where to make change?

[k-okada]

are you using moveit version of angle-vector? if so, does moveit returns including current position as a first point of the planned trajectory? if so, how about using (cdr plannd-trajectory) ?

…

-- ◉ Kei Okada 2016-12-05 19:47 GMT+09:00 pazeshun <notifications@github.com>:

@k-okada <https://github.com/k-okada> Do you have any ideas where to make change? — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#738 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAeG3D1Ev6XR8ONRxcoh8sgkhyGO6n6Zks5rE-vBgaJpZM4LD3L0> .

[pazeshun]

are you using moveit version of angle-vector?

No.

does moveit returns including current position as a first point of the planned trajectory?

I confirmed moveit returns current position as the first point, but I think that position comes from joint_states.

[pazeshun]

This will be fixed in RethinkRobotics/baxter_interface#77. Temporarily, wkentaro/baxter_interface#1 is fine.

[pazeshun]

Closed via wkentaro/baxter_interface#1