def joint_to_robot_state(joint_state, robot_state=None): """ Converts joint state to robot state filling missing joints from the current or given robot state. @joint_state: JointState to convert @robot_state: Optional RobotState that is used to fill missing values, if None current state is used. """ # Get robot state if empty if robot_state is None: robot_state = get_robot_state() # Convert joint state to dictionary js_dict = dict( (x, y) for x, y in zip(joint_state.name, joint_state.position)) # Fill joint_state values into robot_state rs = RobotState() for i, k in enumerate(robot_state.joint_state.name): if k in js_dict: rs.joint_state.name.append(k) rs.joint_state.position.append(js_dict[k]) rs.joint_state.velocity.append(0) else: rs.joint_state.name.append(k) rs.joint_state.position.append(robot_state.joint_state.position[i]) rs.joint_state.velocity.append(robot_state.joint_state.velocity[i]) rs.multi_dof_joint_state = robot_state.multi_dof_joint_state rs.attached_collision_objects = robot_state.attached_collision_objects return rs
def get_robot_state(self): # pose = pose_from_trans(self.get_world_pose(BASE_FRAME)) # TODO: could get this transform directly # transform = Transform(Vector3(*point_from_pose(pose)), Quaternion(*quat_from_pose(pose))) # transform = self.get_transform() # if transform is None: # return None state = RobotState() state.joint_state = self.joint_state # state.multi_dof_joint_state.header.frame_id = '/base_footprint' # state.multi_dof_joint_state.header.stamp = rospy.Time(0) # state.multi_dof_joint_state.joints = ['world_joint'] # state.multi_dof_joint_state.transforms = [transform] # 'world_joint' # http://cram-system.org/tutorials/intermediate/moveit state.attached_collision_objects = [] state.is_diff = False # rostopic info /joint_states return state