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()