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 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
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
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
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
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()
