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