Exemplo n.º 1
0
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)
Exemplo n.º 2
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 = 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
Exemplo 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 = 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
Exemplo n.º 4
0
    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)
Exemplo n.º 5
0
    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)