Esempio n. 1
0
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
Esempio n. 2
0
    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)
Esempio n. 3
0
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())
Esempio n. 5
0
    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)