forked from frankaemika/franka_ros
-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat: add gazebo 'set_model_config' problem example scripts
- Loading branch information
Showing
2 changed files
with
170 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,77 @@ | ||
#!/usr/bin/env python | ||
"""Small example script to show that the joint position is sometimes pushed outside of | ||
the joint limit when the Gazebo 'set_model_configuration' service is used. This script | ||
periodically checks whether the joint positions are within the joint limits and logs a | ||
warning if not. This script is meant to be run together with the | ||
`set_random_joint_positions.py`` script. | ||
""" # noqa: E501 | ||
|
||
import numpy as np | ||
import rospy | ||
from sensor_msgs.msg import JointState | ||
|
||
# Robot info. | ||
JOINT_NAMES = [ | ||
"panda_joint1", | ||
"panda_joint2", | ||
"panda_joint3", | ||
"panda_joint4", | ||
"panda_joint5", | ||
"panda_joint6", | ||
"panda_joint7", | ||
"panda_finger_joint1", | ||
"panda_finger_joint2", | ||
] | ||
JOINT_LIMITS = { | ||
"min": [-2.8973, -1.7628, -2.8973, -3.071, -2.8973, -0.0175, -2.8973, 0.0, 0.0], | ||
"max": [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973, 0.04, 0.04], | ||
} | ||
|
||
|
||
def check_joint_violations(): | ||
"""Check if the joint positions are within the joint limits and log a warning if | ||
not. | ||
""" | ||
try: | ||
joint_state = rospy.wait_for_message("/joint_states", JointState) | ||
except rospy.exceptions.ROSException as e: | ||
rospy.logerr("Failed to receive joint state message: %s", e) | ||
return | ||
|
||
violated_joints = { | ||
JOINT_NAMES[i]: joint_position | ||
for i, joint_position in enumerate(joint_state.position) | ||
if ( | ||
joint_position < JOINT_LIMITS["min"][i] | ||
or joint_position > JOINT_LIMITS["max"][i] | ||
) | ||
} | ||
if violated_joints: | ||
rospy.logwarn( | ||
"The following joints are outside of the joint limits: %s - %s", | ||
list(violated_joints.keys()), | ||
list(violated_joints.values()), | ||
) | ||
else: | ||
rospy.loginfo("No joints are outside of the joint limits.") | ||
|
||
|
||
if __name__ == "__main__": | ||
rospy.init_node("show_gazebo_set_model_configuration_problem") | ||
|
||
# Checks whether the joint positions are within the joint limits every seconds. | ||
rate = rospy.Rate(1) | ||
while not rospy.is_shutdown(): | ||
joint_positions = [ | ||
np.random.uniform(low, high) | ||
for low, high in zip(JOINT_LIMITS["min"], JOINT_LIMITS["max"]) | ||
] | ||
try: | ||
check_joint_violations() | ||
except rospy.exceptions.ROSException as e: | ||
rospy.logerr("Failed to check joint voilations: %s", e) | ||
rate.sleep() | ||
|
||
# Gracefully shut down the node. | ||
rospy.loginfo("Shutting down...") | ||
rospy.signal_shutdown("Finished") |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,93 @@ | ||
#!/usr/bin/env python | ||
|
||
"""Small example script to show that the joint position is sometimes pushed outside of | ||
the joint limit when the Gazebo 'set_model_configuration' service is used. This script | ||
periodically sets random joint positions within the joint limits. This script is meant | ||
to be run together with the `check_joint_violations.py`` script. | ||
""" # noqa: E501 | ||
|
||
import numpy as np | ||
import rospy | ||
from gazebo_msgs.srv import SetModelConfiguration, SetModelConfigurationRequest | ||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint | ||
|
||
# Robot info. | ||
JOINT_NAMES = [ | ||
"panda_joint1", | ||
"panda_joint2", | ||
"panda_joint3", | ||
"panda_joint4", | ||
"panda_joint5", | ||
"panda_joint6", | ||
"panda_joint7", | ||
"panda_finger_joint1", | ||
"panda_finger_joint2", | ||
] | ||
JOINT_LIMITS = { | ||
"min": [-2.8973, -1.7628, -2.8973, -3.071, -2.8973, -0.0175, -2.8973, 0.0, 0.0], | ||
"max": [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973, 0.04, 0.04], | ||
} | ||
JOINT_INITIAL_POSITIONS = [0.0] * len(JOINT_NAMES) | ||
|
||
|
||
def set_random_joint_positions(joint_positions): | ||
"""Set the joint positions of the robot using the Gazebo 'set_model_configuration' | ||
service. | ||
Args: | ||
joint_positions (list): A list of joint positions. | ||
""" | ||
try: | ||
set_model_configuration = rospy.ServiceProxy( | ||
"/gazebo/set_model_configuration", SetModelConfiguration | ||
) | ||
request = SetModelConfigurationRequest() | ||
request.model_name = "panda" | ||
request.urdf_param_name = "robot_description" | ||
request.joint_names = JOINT_NAMES | ||
request.joint_positions = joint_positions | ||
set_model_configuration(request) | ||
rospy.loginfo("Set joint positions to: %s", joint_positions) | ||
except rospy.ServiceException as e: | ||
rospy.logerr("Failed to call set_model_configuration service: %s", e) | ||
|
||
|
||
if __name__ == "__main__": | ||
rospy.init_node("set_random_joint_positions") | ||
|
||
# Set random joint positions within the joint limits. | ||
try: | ||
joint_positions = [ | ||
np.random.uniform(low, high) | ||
for low, high in zip(JOINT_LIMITS["min"], JOINT_LIMITS["max"]) | ||
] | ||
set_random_joint_positions(joint_positions) | ||
except rospy.exceptions.ROSException as e: | ||
rospy.logerr("Failed to set random joint positions: %s", e) | ||
|
||
# Repeat the above every 5 seconds. | ||
rate = rospy.Rate(0.2) | ||
while not rospy.is_shutdown(): | ||
rate.sleep() | ||
try: | ||
joint_positions = [ | ||
np.random.uniform(low, high) | ||
for low, high in zip(JOINT_LIMITS["min"], JOINT_LIMITS["max"]) | ||
] | ||
set_random_joint_positions(joint_positions) | ||
except rospy.exceptions.ROSException as e: | ||
rospy.logerr("Failed to set random joint positions: %s", e) | ||
|
||
# Reset the joint positions to their initial values when the node is shut down. | ||
joint_trajectory = JointTrajectory() | ||
joint_trajectory.joint_names = JOINT_NAMES | ||
joint_trajectory.points.append( | ||
JointTrajectoryPoint(positions=JOINT_INITIAL_POSITIONS) | ||
) | ||
try: | ||
pub = rospy.Publisher( | ||
"/panda_arm_controller/command", JointTrajectory, queue_size=10 | ||
) | ||
pub.publish(joint_trajectory) | ||
except rospy.exceptions.ROSException as e: | ||
rospy.logerr("Failed to reset joint positions: %s", e) |