Skip to content

Commit

Permalink
feat: add gazebo 'set_model_config' problem example scripts
Browse files Browse the repository at this point in the history
  • Loading branch information
rickstaa committed Nov 20, 2023
1 parent 718314b commit c09abfa
Show file tree
Hide file tree
Showing 2 changed files with 170 additions and 0 deletions.
77 changes: 77 additions & 0 deletions franka_gazebo/scripts/log_joint_violations.py
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")
93 changes: 93 additions & 0 deletions franka_gazebo/scripts/set_random_joint_positions.py
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)

0 comments on commit c09abfa

Please sign in to comment.