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())
Exemple #2
0
def getPoseLCMFromXYZRPY(xyz, rpy):

    wxyz = transformUtils.rollPitchYawToQuaternion(rpy)

    trans = bot_core.vector_3d_t()
    trans.x, trans.y, trans.z = xyz

    quat = bot_core.quaternion_t()
    quat.w, quat.x, quat.y, quat.z = wxyz

    pose = bot_core.position_3d_t()
    pose.translation = trans
    pose.rotation = quat

    return pose
Exemple #3
0
def getPoseLCMFromXYZRPY(xyz, rpy):

    wxyz = transformUtils.rollPitchYawToQuaternion(rpy)

    trans = bot_core.vector_3d_t()
    trans.x, trans.y, trans.z = xyz

    quat = bot_core.quaternion_t()
    quat.w, quat.x, quat.y, quat.z = wxyz

    pose = bot_core.position_3d_t()
    pose.translation = trans
    pose.rotation = quat

    return pose
Exemple #4
0
def positionMessageFromFrame(transform):
    '''
    Given a vtkTransform, returns an bot_core.position_t message
    '''

    pos, wxyz = transformUtils.poseFromTransform(transform)

    trans = bot_core.vector_3d_t()
    trans.x, trans.y, trans.z = pos

    quat = bot_core.quaternion_t()
    quat.w, quat.x, quat.y, quat.z = wxyz

    pose = bot_core.position_3d_t()
    pose.translation = trans
    pose.rotation = quat
    return pose
Exemple #5
0
def positionMessageFromFrame(transform):
    '''
    Given a vtkTransform, returns an bot_core.position_t message
    '''

    pos, wxyz = transformUtils.poseFromTransform(transform)

    trans = bot_core.vector_3d_t()
    trans.x, trans.y, trans.z = pos

    quat = bot_core.quaternion_t()
    quat.w, quat.x, quat.y, quat.z = wxyz

    pose = bot_core.position_3d_t()
    pose.translation = trans
    pose.rotation = quat
    return pose
Exemple #6
0
		footsteps = [] # Unpack footstps into list
		for f in plan.footsteps:
			footsteps.append([f.pos.translation.x, f.pos.translation.y])
		footsteps = np.array(footsteps)
		p = Plotter(2, GOAL_POS, footsteps, plan.num_steps/2, REACHABLE, Y_OFFSET)
		p.plot()
		print("-------------------------------------------")
		p.show()

	# Subscribe listener
	subscription = lc.subscribe("FOOTSTEP_PLAN", plan_handle)

	# Publish footstep_plan_request_t 
	request = drc.footstep_plan_request_t()
	
	goalpos = bot_core.position_3d_t()
	goalpos.translation.x = GOAL_POS[0]
	goalpos.translation.y = GOAL_POS[1]
	request.goal_pos = goalpos

	startstate = bot_core.robot_state_t()
	startstate.pose.translation.x = START_POS[0]
	startstate.pose.translation.y = START_POS[1]
	request.initial_state = startstate

	lc.publish("REQUEST", request.encode())

	# Listen for response with footstep_plan_t
	try:
		while True:
			lc.handle()