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 = lcmdrc.robot_state_t()
m.utime = int(time.time() * 1e6)
m.pose = getPoseLCMFromXYZRPY(xyz, rpy)
m.twist = lcmdrc.twist_t()
m.twist.linear_velocity = lcmdrc.vector_3d_t()
m.twist.angular_velocity = lcmdrc.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 = lcmdrc.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 state_handle(self, channel, data):
msg = drc.robot_state_t.decode(data)
msg_out = drc.robot_state_t()
msg_out.utime = 1e6*time.time()
msg_out.joint_name = msg.joint_name
msg_out.num_joints = msg.num_joints
msg_out.joint_effort = [0]*msg.num_joints
msg_out.joint_position = [0]*msg.num_joints
msg_out.joint_velocity = [0]*msg.num_joints
msg_out.force_torque = drc.force_torque_t()
msg_out.pose = drc.position_3d_t()
msg_out.pose.translation = drc.vector_3d_t()
msg_out.pose.rotation = drc.quaternion_t()
msg_out.twist = drc.twist_t()
msg_out.twist.linear_velocity = drc.vector_3d_t()
msg_out.twist.angular_velocity = drc.vector_3d_t()
for i in range(0,msg.num_joints):
torque_lims = self.get_torque_lims(msg.joint_name[i],msg.joint_position[i])
position_lims = self.get_joint_lims(msg.joint_name[i])
# print(position_lims)
msg_out.joint_effort[i] = (min(100,max(-100,-1 + 2*(msg.joint_effort[i] - torque_lims[0])/(torque_lims[1] - torque_lims[0]))))
msg_out.joint_position[i] = (min(100,max(-100,-1 + 2*(msg.joint_position[i] - position_lims[0])/(position_lims[1] - position_lims[0]))))
# print("lims: " + str(torque_lims) + " torque: " + str(msg.joint_effort[i]) + " scaled: " + str(msg_out.joint_effort[i]))
# print("jlims: " + str(position_lims) + " position: " + str(msg.joint_position[i]) + " scaled: " + str(msg_out.joint_position[i]))
lc.publish("SCALED_ROBOT_STATE", msg_out.encode())
time.sleep(.01)
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 = lcmdrc.robot_state_t()
m.utime = int(time.time() * 1e6)
m.pose = getPoseLCMFromXYZRPY(xyz, rpy)
m.twist = lcmdrc.twist_t()
m.twist.linear_velocity = lcmdrc.vector_3d_t()
m.twist.angular_velocity = lcmdrc.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 = lcmdrc.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 convertNSend(self,channel,data):
msgIn = abblcm.abb_irb140state.decode(data)
msgOut = drc.robot_state_t()
### Msg Conversion
msgOut.utime = msgIn.utime
msgOut.pose = drc.position_3d_t()
msgOut.pose.translation = drc.vector_3d_t()
msgOut.pose.translation.x = 0.0
msgOut.pose.translation.y = 0.0
msgOut.pose.translation.z = 0.0
msgOut.pose.rotation = drc.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 = drc.twist_t()
msgOut.twist.linear_velocity = drc.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 = drc.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 = drc.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 state_handle(self, channel, data):
msg = drc.robot_state_t.decode(data)
msg_out = drc.robot_state_t()
msg_out.utime = 1e6 * time.time()
msg_out.joint_name = msg.joint_name
msg_out.num_joints = msg.num_joints
msg_out.joint_effort = [0] * msg.num_joints
msg_out.joint_position = [0] * msg.num_joints
msg_out.joint_velocity = [0] * msg.num_joints
msg_out.force_torque = drc.force_torque_t()
msg_out.pose = drc.position_3d_t()
msg_out.pose.translation = drc.vector_3d_t()
msg_out.pose.rotation = drc.quaternion_t()
msg_out.twist = drc.twist_t()
msg_out.twist.linear_velocity = drc.vector_3d_t()
msg_out.twist.angular_velocity = drc.vector_3d_t()
for i in range(0, msg.num_joints):
torque_lims = self.get_torque_lims(msg.joint_name[i],
msg.joint_position[i])
position_lims = self.get_joint_lims(msg.joint_name[i])
# print(position_lims)
msg_out.joint_effort[i] = (min(
100,
max(
-100, -1 + 2 * (msg.joint_effort[i] - torque_lims[0]) /
(torque_lims[1] - torque_lims[0]))))
msg_out.joint_position[i] = (min(
100,
max(
-100, -1 + 2 * (msg.joint_position[i] - position_lims[0]) /
(position_lims[1] - position_lims[0]))))
# print("lims: " + str(torque_lims) + " torque: " + str(msg.joint_effort[i]) + " scaled: " + str(msg_out.joint_effort[i]))
# print("jlims: " + str(position_lims) + " position: " + str(msg.joint_position[i]) + " scaled: " + str(msg_out.joint_position[i]))
lc.publish("SCALED_ROBOT_STATE", msg_out.encode())
time.sleep(.01)
