def onIiwaStatus(msg):
fingerHalfDist = lastGripperMsg.actual_position_mm * 1e-3 * 0.5
fingerJointNames = ['wsg_50_finger_left_joint', 'wsg_50_finger_right_joint']
fingerJointPositions = [fingerHalfDist, fingerHalfDist]
armJointNames = ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7']
armJointPositions = list(msg.joint_position_measured)
jointNames = armJointNames + fingerJointNames
jointPositions = armJointPositions + fingerJointPositions
m = lcmbotcore.robot_state_t()
m.utime = msg.utime
m.pose = robotstate.getPoseLCMFromXYZRPY([0,0,0], [0,0,0])
m.twist = lcmbotcore.twist_t()
m.twist.linear_velocity = lcmbotcore.vector_3d_t()
m.twist.angular_velocity = lcmbotcore.vector_3d_t()
m.num_joints = len(jointNames)
m.joint_name = jointNames
m.joint_position = jointPositions
m.joint_velocity = np.zeros(m.num_joints)
m.joint_effort = np.zeros(m.num_joints)
m.force_torque = lcmbotcore.force_torque_t()
m.force_torque.l_hand_force = np.zeros(3)
m.force_torque.l_hand_torque = np.zeros(3)
m.force_torque.r_hand_force = np.zeros(3)
m.force_torque.r_hand_torque = np.zeros(3)
lcmUtils.publish('EST_ROBOT_STATE', m)
def onIiwaStatus(self, msg):
armJointPositions = list(msg.joint_position_measured)
jointPosition = armJointPositions + self.fingerJointPositions
m = lcmbotcore.robot_state_t()
# m.utime = msg.utime # this used to get utimes from the kuka robot. should later fix in drake-iiwa-driver/src/kuka_driver.cc
m.utime = getUtime()
m.pose = robotstate.getPoseLCMFromXYZRPY(self.basePosition[0:3],
self.basePosition[3:6])
m.twist = lcmbotcore.twist_t()
m.twist.linear_velocity = lcmbotcore.vector_3d_t()
m.twist.angular_velocity = lcmbotcore.vector_3d_t()
m.num_joints = self.numJoints
m.joint_name = self.jointNames
m.joint_position = jointPosition
m.joint_velocity = np.zeros(m.num_joints)
m.joint_effort = np.zeros(m.num_joints)
m.force_torque = lcmbotcore.force_torque_t()
m.force_torque.l_hand_force = np.zeros(3)
m.force_torque.l_hand_torque = np.zeros(3)
m.force_torque.r_hand_force = np.zeros(3)
m.force_torque.r_hand_torque = np.zeros(3)
lcmUtils.publish('EST_ROBOT_STATE', m)
def onIiwaStatus(self, msg):
armJointPositions = list(msg.joint_position_measured)
jointPosition = armJointPositions + self.fingerJointPositions
m = lcmbotcore.robot_state_t()
m.utime = msg.utime
m.pose = robotstate.getPoseLCMFromXYZRPY([0,0,0], [0,0,0])
m.twist = lcmbotcore.twist_t()
m.twist.linear_velocity = lcmbotcore.vector_3d_t()
m.twist.angular_velocity = lcmbotcore.vector_3d_t()
m.num_joints = self.numJoints
m.joint_name = self.jointNames
m.joint_position = jointPosition
m.joint_velocity = np.zeros(m.num_joints)
m.joint_effort = np.zeros(m.num_joints)
m.force_torque = lcmbotcore.force_torque_t()
m.force_torque.l_hand_force = np.zeros(3)
m.force_torque.l_hand_torque = np.zeros(3)
m.force_torque.r_hand_force = np.zeros(3)
m.force_torque.r_hand_torque = np.zeros(3)
lcmUtils.publish('EST_ROBOT_STATE', m)
def send_position():
msg = bot_core.robot_state_t()
msg.pose = robotstate.getPoseLCMFromXYZRPY([0.80, 0.36, 0.29],
[0., 0., 0.])
lcmUtils.publish('OBJECT_STATE_EST', msg)