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)