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 drakePoseToRobotState(drakePose):
robotState = range(getNumJoints())
jointIndexMap = getDrakePoseToRobotStateJointMap()
for jointIdx, jointAngle in enumerate(drakePose):
jointIdx =jointIndexMap.get(jointIdx)
if jointIdx is not None:
robotState[jointIdx] = jointAngle
xyz = drakePose[:3]
rpy = drakePose[3:6]
m = bot_core.robot_state_t()
m.utime = int(time.time() * 1e6)
m.pose = getPoseLCMFromXYZRPY(xyz, rpy)
m.twist = bot_core.twist_t()
m.twist.linear_velocity = bot_core.vector_3d_t()
m.twist.angular_velocity = bot_core.vector_3d_t()
m.num_joints = getNumJoints()
m.joint_name = getRobotStateJointNames()
m.joint_position = robotState
m.joint_velocity = np.zeros(getNumJoints())
m.joint_effort = np.zeros(getNumJoints())
m.force_torque = bot_core.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)
return m
def drakePoseToRobotState(drakePose):
robotState = range(getNumJoints())
jointIndexMap = getDrakePoseToRobotStateJointMap()
for jointIdx, jointAngle in enumerate(drakePose):
jointIdx = jointIndexMap.get(jointIdx)
if jointIdx is not None:
robotState[jointIdx] = jointAngle
xyz = drakePose[:3]
rpy = drakePose[3:6]
m = bot_core.robot_state_t()
m.utime = int(time.time() * 1e6)
m.pose = getPoseLCMFromXYZRPY(xyz, rpy)
m.twist = bot_core.twist_t()
m.twist.linear_velocity = bot_core.vector_3d_t()
m.twist.angular_velocity = bot_core.vector_3d_t()
m.num_joints = getNumJoints()
m.joint_name = getRobotStateJointNames()
m.joint_position = robotState
m.joint_velocity = np.zeros(getNumJoints())
m.joint_effort = np.zeros(getNumJoints())
m.force_torque = bot_core.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)
return 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 convertNSend(self,channel,data):
msgIn = abblcm.abb_irb140state.decode(data)
msgOut = bot_core.robot_state_t()
### Msg Conversion
msgOut.utime = msgIn.utime
msgOut.pose = bot_core.position_3d_t()
msgOut.pose.translation = bot_core.vector_3d_t()
msgOut.pose.translation.x = -0.17 # 0.0 #HACKY FIXME READ FROM irb140.cfg n = [ 0, 0, .911 ]; = [ -0.17, 0, 0 ]; # -.17 , 0, 0 #
msgOut.pose.translation.y = 0.0
msgOut.pose.translation.z = 0.911
msgOut.pose.rotation = bot_core.quaternion_t()
# rotate by x axis by -90 degrees
msgOut.pose.rotation.w = 1.0
msgOut.pose.rotation.x = 0.0
msgOut.pose.rotation.y = 0.0
msgOut.pose.rotation.z = 0.0
msgOut.twist = bot_core.twist_t()
msgOut.twist.linear_velocity = bot_core.vector_3d_t()
msgOut.twist.linear_velocity.x = 0.0
msgOut.twist.linear_velocity.y = 0.0
msgOut.twist.linear_velocity.z = 0.0
msgOut.twist.angular_velocity = bot_core.vector_3d_t()
msgOut.twist.angular_velocity.x = 0.0
msgOut.twist.angular_velocity.y = 0.0
msgOut.twist.angular_velocity.z = 0.0
msgOut.num_joints = len(msgIn.joints.pos)
msgOut.joint_name = ["joint" + str(i+1) for i in range(msgOut.num_joints)]
msgOut.joint_position = [joint_pos/180.0*math.pi for joint_pos in msgIn.joints.pos]
#msgOut.joint_position[0] = -msgOut.joint_position[0]
#msgOut.joint_position[2] = -msgOut.joint_position[2]
msgOut.joint_velocity = [joint_vel/180.0*math.pi for joint_vel in msgIn.joints.vel]
msgOut.joint_effort = [0.0 for i in range(msgOut.num_joints)]
msgOut.force_torque = bot_core.force_torque_t()
msgOut.force_torque.l_foot_force_z = 0
msgOut.force_torque.l_foot_torque_x = 0
msgOut.force_torque.l_foot_torque_y = 0
msgOut.force_torque.r_foot_force_z = 0
msgOut.force_torque.r_foot_torque_x = 0
msgOut.force_torque.r_foot_torque_y = 0
msgOut.force_torque.l_hand_force = [0,0,0]
msgOut.force_torque.l_hand_torque = [0,0,0]
msgOut.force_torque.r_hand_force = [0,0,0]
msgOut.force_torque.r_hand_torque = [0,0,0]
#Msg Publish
self.lc.publish("EST_ROBOT_STATE", msgOut.encode())
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)