Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

reset_to_home.py causes error on pr1040 but not on gazebo simulator #64

Open
HiroIshida opened this issue Sep 4, 2022 · 0 comments · May be fixed by #65
Open

reset_to_home.py causes error on pr1040 but not on gazebo simulator #64

HiroIshida opened this issue Sep 4, 2022 · 0 comments · May be fixed by #65

Comments

@HiroIshida
Copy link
Owner

HiroIshida commented Sep 4, 2022 •
edited
Loading

problem (probably pr2 hardware or pr1040 specific issue)

(This issue is using version after #63)
rosrun mohou_ros reset_to_home.py on real robot
causes the following error

[ERROR] [1662316480.793608]: 1 non-controlling joint angles are not consistent with home position.
{'joint_name': 'head_tilt_joint', 'angle': 1.1361046232931888, 'angle_desired': 1.0728538912009142, 'error': 0.06325073209227461, 'threshold': 0.03490658503988659}

However, in the gazebo simulator, after roslaunch pr2_gazebo pr2_empty_world.launch the same command works without problem.

Also, I conducted additional experiments. running the same command changing the initial tilt angle.
case1: tilt_joint is set to the highest value (neck is pointing to the ground)
case2: tilt_jotin is set to the lowest value (neck is pointing to the sky)
In case2, the command works well, but in case1 the same error occurred.

Also I check the /head_traj_controller/follow_joint_trajectory/feedback topic when testing on case1. And the output is like the following. The output shows that desired neck position is set correctly, but controller cannot drive the joint to the desired angle. So, I conclude that it is a hardware-rooted problem rather than a scikit-robot software issue.

desired: 
    positions: [-9.237630237555333e-05, 1.0728548206764066]
    velocities: [-0.00010230853856615929, -0.0008065962224733658]
    accelerations: [0.0459219098688491, 0.3620464093036396]
    effort: [-0.3858327788697062, -6.280792393178126]
    time_from_start: 
      secs: 1
      nsecs: 997752683
  actual: 
    positions: [0.00734261242871972, 1.1346385467215137]
    velocities: [0.0, 0.0]
    accelerations: []
    effort: [-0.38978100827336315, -6.277091636535645]
    time_from_start: 
      secs: 1
      nsecs: 997752683
  error: 
    positions: [-0.007434988731095274, -0.06178372604510707]
    velocities: [0.00010230853856615929, 0.0008065962224733658]
    accelerations: []
    effort: [-0.0039482294036569665, 0.003700756642481373]
    time_from_start: 
      secs: 1
      nsecs: 997752683

Actually, the same problem occurs when using euslisp. The following code compute the difference between desired and actual joint angle. and output was 3.26 [deg].

(load "package://pr2eus/pr2-interface.l")
(pr2-init)

(setq *desired* 60)
(setq *joint* (send *pr2* :head_tilt_joint))
(send *joint* :joint-angle *desired*)

(send *ri* :angle-vector (send *pr2* :angle-vector))
(send *ri* :wait-interpolation)

(send *pr2* :angle-vector (send *ri* :state :potentio-vector))

(print (- (send *joint* :joint-angle) *desired*))

home position

home_position.yaml

bl_caster_l_wheel_joint: -1747.4960929448357
bl_caster_r_wheel_joint: -1598.6965015232092
bl_caster_rotation_joint: 0.7839066620752853
br_caster_l_wheel_joint: 1070.9315347971171
br_caster_r_wheel_joint: 1234.7029486263784
br_caster_rotation_joint: 2.355731935983192
fl_caster_l_wheel_joint: 2303.9117993989953
fl_caster_r_wheel_joint: 2468.8539379256895
fl_caster_rotation_joint: -0.7855964009142032
fr_caster_l_wheel_joint: -1962.6303188481893
fr_caster_r_wheel_joint: -1812.2672281283776
fr_caster_rotation_joint: 0.7872200605797226
head_pan_joint: -9.249018477611539e-05
head_tilt_joint: 1.0728538912009142
l_elbow_flex_joint: -1.919922381206603
l_forearm_roll_joint: -0.34919022050429266
l_gripper_joint: 0.08644129011040648
l_gripper_l_finger_joint: 0.5043376890868512
l_gripper_l_finger_tip_joint: 0.5043376890868512
l_gripper_motor_screw_joint: 0.0
l_gripper_motor_slider_joint: 0.0
l_gripper_r_finger_joint: 0.5043376890868512
l_gripper_r_finger_tip_joint: 0.5043376890868512
l_shoulder_lift_joint: 0.8726545105347081
l_shoulder_pan_joint: 1.30900678614617
l_upper_arm_roll_joint: 1.9197462652738873
l_wrist_flex_joint: -0.17464744792839326
l_wrist_roll_joint: -0.17455149038717366
laser_tilt_mount_joint: 0.4299841839399005
r_elbow_flex_joint: -2.0506503335859287
r_forearm_roll_joint: 4.584582902038177
r_gripper_joint: 0.08704873817414047
r_gripper_l_finger_joint: 0.5082783783631448
r_gripper_l_finger_tip_joint: 0.5082783783631448
r_gripper_motor_screw_joint: 0.0
r_gripper_motor_slider_joint: 0.0
r_gripper_r_finger_joint: 0.5082783783631448
r_gripper_r_finger_tip_joint: 0.5082783783631448
r_shoulder_lift_joint: 0.10846794379949935
r_shoulder_pan_joint: -0.6483372056934948
r_upper_arm_roll_joint: -1.1107506377386362
r_wrist_flex_joint: -1.9900264969748427
r_wrist_roll_joint: -3.3873396124245696
torso_lift_joint: 0.29982514252114517
torso_lift_motor_screw_joint: 0.0
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging a pull request may close this issue.

fix: ad-hoc workaround for pr1040 tilt_joint controller issue HiroIshida/mohou_ros
1 participant
@HiroIshida