class PandaRosBridge:
def __init__(self, real_robot=False):
# Event is clear while initialization or set_state is going on
self.reset = Event()
self.reset.clear()
self.get_state_event = Event()
self.get_state_event.set()
self.real_robot = real_robot
# TODO publisher, subscriber, target and state
self._add_publishers()
self.panda_arm = ArmInterface()
self.target = [0.0] * 6 # TODO define number of target floats
# TODO define number of panda states (At least the number of joints)
self.panda_joint_names = [
'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4',
'panda_joint5', 'panda_joint6', 'panda_joint7'
]
self.panda_finger_names = [
'panda_finger_joint1', 'panda_finger_joint2'
]
self.panda_joint_num = len(self.panda_joint_names)
self.panda_state = [0.0] * self.panda_joint_num
# TF Listener
self.tf_listener = tf.TransformListener()
# TF2 Listener
# self.tf2_buffer = tf2_ros.Buffer()
# self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer)
# Static TF2 Broadcaster
# self.static_tf2_broadcaster = tf2_ros.StaticTransformBroadcaster()
# Robot control rate
self.sleep_time = (1.0 / rospy.get_param('~action_cycle_rate')) - 0.01
self.control_period = rospy.Duration.from_sec(self.sleep_time)
self.reference_frame = rospy.get_param('~reference_frame', 'base')
self.ee_frame = 'tool0' # TODO is the value for self.ee_frame correct?
self.target_frame = 'target'
# Minimum Trajectory Point time from start
# TODO check if this value is correct for the panda robot
self.min_traj_duration = 0.5
if not self.real_robot:
# Subscribers to link collision sensors topics
# TODO add rospy.Subscribers
rospy.Subscriber('/joint_states', JointState, self.callback_panda)
# TODO add keys to collision sensors
self.collision_sensors = dict.fromkeys([], False)
# TODO currently not used
self.safe_to_move = True
# Target mode
self.target_mode = rospy.get_param('~target_mode', FIXED_TARGET_MODE)
self.target_mode_name = rospy.get_param('~target_model_name', 'box100')
# Object parameters
# self.objects_controller = rospy.get_param('objects_controller', False)
# self.n_objects = int(rospy.get_param('n_objects', 0))
# if self.objects_controller:
# self.objects_model_name = []
# for i in range(self.n_objects):
# obj_model_name = rospy.get_param(
# 'object_' + repr(i) + '_model_name')
# self.objects_model_name.append(obj_model_name)
def callback_panda(self, data):
"""Callback function which sets the panda state
- `data.name` -> joint names
- `data.position` -> current joint positions
- `data.velocity` -> current velocity of joints
- `data.effort` -> current torque (effort) of joints
Args:
`data` (JointStates): data containing the current joint states
"""
# TODO gripper might also be included in this state
# if self.get_state_event.is_set():
self.panda_state[0:7] = data.position[0:7]
self.panda_state[7:14] = data.velocity[0:7]
self.panda_state[14:21] = data.effort[0:7]
def _add_publishers(self):
"""Adds publishers to ROS bridge
- joint trajectory command handler
- RViz target marker
"""
# joint_trajectory_command handler publisher
self.arm_cmd_pub = rospy.Publisher('env_arm_command',
JointTrajectory,
queue_size=1)
# Target RViz Marker publisher
self.target_pub = rospy.Publisher('target_marker',
Marker,
queue_size=10)
def get_state(self):
self.get_state_event.clear()
# # currently only working on a fixed target mode
if self.target_mode == FIXED_TARGET_MODE:
target = copy.deepcopy(self.target)
else:
raise ValueError
panda_state = copy.deepcopy(self.panda_state)
# # TODO is ee_to_base_transform value correctly loaded and set
# (position, quaternion) = self.tf_listener.lookupTransform(
# self.reference_frame)
# ee_to_base_transform = position + quaternion
# # TODO currently not needed
# # if self.real_robot:
# # panda_collision = False
# # else:
# # panda_collision = any(self.collision_sensors.values())
self.get_state_event.set()
# Create and fill State message
msg = robot_server_pb2.State()
msg.state.extend(target)
msg.state.extend(panda_state)
# msg.state.extend(ee_to_base_transform)
# msg.state.extend([panda_collision])
msg.success = True
return msg
def set_state(self, state_msg):
# Set environment state
state = state_msg.state
self.reset.clear()
# Set target internal value
if self.target_mode == FIXED_TARGET_MODE:
# TODO found out how many state values are needed for panda
self.target = copy.deepcopy(state[0:6])
# Publish Target Marker
self.publish_target_marker(self.target)
# TODO Broadcast target tf2
# TODO setup objects movement
# if self.objects_controller:
transformed_j_pos = self._transform_panda_list_to_dict(state[6:13])
reset_steps = int(15.0 / self.sleep_time)
for _ in range(reset_steps):
self.panda_arm.set_joint_positions(transformed_j_pos)
self.reset.set()
rospy.sleep(self.control_period)
return True
def publish_env_arm_cmd(self, position_cmd):
"""Publish environment JointTrajectory msg.
Publish JointTrajectory message to the env_command topic.
Args:
position_cmd (type): Description of parameter `positions`.
Returns:
type: Description of returned object.
"""
# if self.safe_to_move:
# msg = JointTrajectory()
# msg.header = Header()
# msg.joint_names = copy.deepcopy(self.panda_joint_names)
# msg.points = [JointTrajectoryPoint()]
# msg.points[0].positions = position_cmd
# duration = []
# for i in range(len(msg.joint_names)):
# TODO check if the index is in bounds
# !!! Be careful with panda_state index here
# pos = self.panda_state[i]
# cmd = position_cmd[i]
# max_vel = self.panda_joint_vel_limits[i]
# temp_duration = max(abs(cmd - pos) / max_vel, self.min_traj_duration)
# duration.append(temp_duration)
# msg.points[0].time_from_start = rospy.Duration.from_sec(max(duration))
# print(msg)
# self.arm_cmd_pub.publish(msg)
if self.safe_to_move:
transformed_j_pos = self._transform_panda_list_to_dict(
position_cmd[0:7])
self.panda_arm.set_joint_positions(transformed_j_pos)
rospy.sleep(self.control_period)
return position_cmd
else:
rospy.sleep(self.control_period)
return [0.0] * self.panda_joint_num
def get_model_state_pose(self, model_name, relative_entity_name=''):
# method used to retrieve model pose from gazebo simulation
rospy.wait_for_service('/gazebo/get_model_state')
try:
model_state = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
s = model_state(model_name, relative_entity_name)
pose = [s.pose.position.x, s.pose.position.y, s.pose.position.z, \
s.pose.orientation.x, s.pose.orientation.y, s.pose.orientation.z, s.pose.orientation.w]
return pose
except rospy.ServiceException as e:
print("Service call failed:" + e)
def get_link_state(self, link_name, reference_frame=''):
"""Method is used to retrieve link state from gazebo simulation
Args:
link_name (name of the link): [description]
reference_frame (str, optional): [description]. Defaults to ''.
Returns:
state of the link corresponding to the given name ->
[x_pos, y_pos, z_pos, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel]
"""
gazebo_get_link_state_service = '/gazebo/get_link_state'
rospy.wait_for_service(gazebo_get_link_state_service)
try:
link_state_srv = rospy.ServiceProxy(gazebo_get_link_state_service,
GetLinkState)
link_coordinates = link_state_srv(link_name,
reference_frame).link_state
link_pose, link_twist = link_coordinates.pose, link_coordinates.twist
x_pos = link_pose.position.x
y_pos = link_pose.position.y
z_pos = link_pose.position.z
orientation = PyKDL.Rotation.Quaternion(link_pose.orientation.x,
link_pose.orientation.y,
link_pose.orientation.z,
link_pose.orientation.w)
euler_orientation = orientation.GetRPY()
roll = euler_orientation[0]
pitch = euler_orientation[1]
yaw = euler_orientation[2]
x_vel = link_twist.linear.x
y_vel = link_twist.linear.y
z_vel = link_twist.linear.z
roll_vel = link_twist.angular.x
pitch_vel = link_twist.angular.y
yaw_vel = link_twist.angular.z
return x_pos, y_pos, z_pos, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel
except rospy.ServiceException as err:
error_message = 'Service call failed: ' + err
rospy.logerr(error_message)
print(error_message)
def publish_target_marker(self, target_pose):
t_marker = Marker()
t_marker.type = 1 # =>CUBE
t_marker.action = 0
t_marker.frame_locked = 1
t_marker.pose.position.x = target_pose[0]
t_marker.pose.position.y = target_pose[1]
t_marker.pose.position.z = target_pose[2]
rpy_orientation = PyKDL.Rotation.RPY(target_pose[3], target_pose[4],
target_pose[5])
q_orientation = rpy_orientation.GetQuaternion()
t_marker.pose.orientation.x = q_orientation[0]
t_marker.pose.orientation.y = q_orientation[1]
t_marker.pose.orientation.z = q_orientation[2]
t_marker.pose.orientation.w = q_orientation[3]
t_marker.scale.x = 0.1
t_marker.scale.y = 0.1
t_marker.scale.z = 0.1
t_marker.id = 0
t_marker.header.stamp = rospy.Time.now()
t_marker.header.frame_id = self.reference_frame
t_marker.color.a = 0.7
t_marker.color.r = 1.0 # red
t_marker.color.g = 0.0
t_marker.color.b = 0.0
self.target_pub.publish(t_marker)
def _transform_panda_list_to_dict(self, panda_list):
transformed_dict = {}
for idx, value in enumerate(panda_list):
current_joint_name = self.panda_joint_names[idx]
transformed_dict[current_joint_name] = value
return transformed_dict
initial_pose = deepcopy(r.joint_ordered_angles())
input("Hit Enter to Start")
print("commanding")
vals = deepcopy(initial_pose)
count = 0
while not rospy.is_shutdown():
elapsed_time_ += period
delta = 3.14 / 16.0 * (
1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2
for j, _ in enumerate(vals):
if j == 4:
vals[j] = initial_pose[j] - delta
else:
vals[j] = initial_pose[j] + delta
if count % 500 == 0:
print(vals, delta)
print("\n ---- \n")
print(" ")
# r.set_joint_positions_velocities(vals, [0.0 for _ in range(7)]) # for impedance control
r.set_joint_positions(dict(zip(r.joint_names(),
vals))) # try this for position control
count += 1
rate.sleep()