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 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 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 to_footstep_t(self):
msg = drc.footstep_t()
msg.pos = drc.position_3d_t()
msg.pos.translation = drc.vector_3d_t()
msg.pos.translation.x, msg.pos.translation.y, msg.pos.translation.z = self.pos[:3]
msg.pos.rotation = drc.quaternion_t()
msg.pos.rotation.w, msg.pos.rotation.x, msg.pos.rotation.y, msg.pos.rotation.z = ut.rpy2quat(self.pos[3:])
msg.id = self.step_id
msg.is_right_foot = self.is_right_foot
msg.is_in_contact = self.is_in_contact
msg.fixed_x, msg.fixed_y, msg.fixed_z, msg.fixed_roll, msg.fixed_pitch, msg.fixed_yaw = self.pos_fixed
msg.num_terrain_pts = self.terrain_pts.shape[1]
if msg.num_terrain_pts > 0:
msg.terrain_path_dist = self.terrain_pts[0, :]
msg.terrain_height = self.terrain_pts[1, :]
msg.params = drc.footstep_params_t()
# TODO: this should probably be filled in
msg.params.bdi_step_duration = self.bdi_step_duration
msg.params.bdi_sway_duration = self.bdi_sway_duration
msg.params.bdi_lift_height = self.bdi_lift_height
msg.params.bdi_toe_off = self.bdi_toe_off
msg.params.bdi_knee_nominal = self.bdi_knee_nominal
msg.params.bdi_max_body_accel = self.bdi_max_body_accel
msg.params.bdi_max_foot_vel = self.bdi_max_foot_vel
msg.params.bdi_sway_end_dist = self.bdi_sway_end_dist
msg.params.bdi_step_end_dist = self.bdi_step_end_dist
msg.params.support_contact_groups = self.support_contact_groups
return msg
def to_footstep_t(self):
msg = drc.footstep_t()
msg.pos = drc.position_3d_t()
msg.pos.translation = drc.vector_3d_t()
msg.pos.translation.x, msg.pos.translation.y, msg.pos.translation.z = self.pos[:
3]
msg.pos.rotation = drc.quaternion_t()
msg.pos.rotation.w, msg.pos.rotation.x, msg.pos.rotation.y, msg.pos.rotation.z = ut.rpy2quat(
self.pos[3:])
msg.id = self.step_id
msg.is_right_foot = self.is_right_foot
msg.is_in_contact = self.is_in_contact
msg.fixed_x, msg.fixed_y, msg.fixed_z, msg.fixed_roll, msg.fixed_pitch, msg.fixed_yaw = self.pos_fixed
msg.num_terrain_pts = self.terrain_pts.shape[1]
if msg.num_terrain_pts > 0:
msg.terrain_path_dist = self.terrain_pts[0, :]
msg.terrain_height = self.terrain_pts[1, :]
msg.params = drc.footstep_params_t()
# TODO: this should probably be filled in
msg.params.bdi_step_duration = self.bdi_step_duration
msg.params.bdi_sway_duration = self.bdi_sway_duration
msg.params.bdi_lift_height = self.bdi_lift_height
msg.params.bdi_toe_off = self.bdi_toe_off
msg.params.bdi_knee_nominal = self.bdi_knee_nominal
msg.params.bdi_max_body_accel = self.bdi_max_body_accel
msg.params.bdi_max_foot_vel = self.bdi_max_foot_vel
msg.params.bdi_sway_end_dist = self.bdi_sway_end_dist
msg.params.bdi_step_end_dist = self.bdi_step_end_dist
msg.params.support_contact_groups = self.support_contact_groups
return msg
def generate_deprecated_plan(self, behavior):
plan = drc.deprecated_footstep_plan_t()
plan.utime = 0
plan.robot_name = 'atlas'
plan.num_steps = 10
plan.is_new_plan = True
plan.footstep_opts = drc.footstep_opts_t()
plan.footstep_opts.mu = True
plan.footstep_opts.behavior = behavior
for j in range(plan.num_steps):
goal = drc.footstep_goal_t()
goal.utime = 0
goal.robot_name = 'atlas'
goal.pos = drc.position_3d_t()
goal.pos.translation = drc.vector_3d_t()
if 1 <= j <= 2: # this works in Python!
goal.pos.translation.x = 0
goal.pos.translation.y = 0
goal.pos.translation.z = 0
goal.pos.rotation = drc.quaternion_t()
goal.pos.rotation.x = 0
goal.pos.rotation.y = 0
goal.pos.rotation.z = 0
goal.pos.rotation.w = 1
else:
goal.pos.translation.x = 0.15 * j + (0.5 -
random.random()) * 0.15
goal.pos.translation.y = random.random() * 0.15 * j + (
0.5 - random.random()) * 0.2
goal.pos.translation.z = (0.5 - random.random()) * 0.2
goal.pos.rotation = drc.quaternion_t()
goal.pos.rotation.x = random.random()
goal.pos.rotation.y = random.random()
goal.pos.rotation.z = random.random()
goal.pos.rotation.w = random.random()
goal.step_speed = 1.5
goal.step_height = random.random() * 0.25
goal.id = j + 1
goal.is_right_foot = j % 2 == 0
goal.is_in_contact = True
goal.fixed_x = True
goal.fixed_y = True
goal.fixed_z = True
goal.fixed_roll = True
goal.fixed_pitch = True
goal.fixed_yaw = True
goal.bdi_step_duration = 2.0
goal.bdi_sway_duration = 0
goal.bdi_lift_height = random.random() * 0.15
goal.bdi_toe_off = 0
goal.bdi_knee_nominal = 0
goal.num_terrain_pts = 3
goal.terrain_path_dist = [0, 0.1, 0.2]
goal.terrain_height = [0, random.random() * 0.15, 0]
plan.footstep_goals.append(goal)
return plan
def generate_deprecated_plan(self, behavior):
plan = drc.deprecated_footstep_plan_t()
plan.utime = 0
plan.robot_name = 'atlas'
plan.num_steps = 10
plan.is_new_plan = True
plan.footstep_opts = drc.footstep_opts_t()
plan.footstep_opts.mu = True
plan.footstep_opts.behavior = behavior
for j in range(plan.num_steps):
goal = drc.footstep_goal_t()
goal.utime = 0
goal.robot_name = 'atlas'
goal.pos = drc.position_3d_t();
goal.pos.translation = drc.vector_3d_t()
if 1 <= j <= 2: # this works in Python!
goal.pos.translation.x = 0
goal.pos.translation.y = 0
goal.pos.translation.z = 0
goal.pos.rotation = drc.quaternion_t()
goal.pos.rotation.x = 0
goal.pos.rotation.y = 0
goal.pos.rotation.z = 0
goal.pos.rotation.w = 1
else:
goal.pos.translation.x = 0.15 * j + (0.5 - random.random()) * 0.15
goal.pos.translation.y = random.random() * 0.15 * j + (0.5 - random.random()) * 0.2
goal.pos.translation.z = (0.5 - random.random()) * 0.2
goal.pos.rotation = drc.quaternion_t()
goal.pos.rotation.x = random.random()
goal.pos.rotation.y = random.random()
goal.pos.rotation.z = random.random()
goal.pos.rotation.w = random.random()
goal.step_speed = 1.5
goal.step_height = random.random() * 0.25
goal.id = j+1
goal.is_right_foot = j % 2 == 0
goal.is_in_contact = True
goal.fixed_x = True
goal.fixed_y = True
goal.fixed_z = True
goal.fixed_roll = True
goal.fixed_pitch= True
goal.fixed_yaw = True
goal.bdi_step_duration = 2.0
goal.bdi_sway_duration = 0
goal.bdi_lift_height = random.random() * 0.15
goal.bdi_toe_off = 0
goal.bdi_knee_nominal = 0
goal.num_terrain_pts = 3
goal.terrain_path_dist = [0, 0.1, 0.2]
goal.terrain_height = [0, random.random() * 0.15, 0]
plan.footstep_goals.append(goal)
return plan
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 getPoseLCMFromXYZRPY(xyz, rpy):
wxyz = transformUtils.rollPitchYawToQuaternion(rpy)
trans = lcmdrc.vector_3d_t()
trans.x, trans.y, trans.z = xyz
quat = lcmdrc.quaternion_t()
quat.w, quat.x, quat.y, quat.z = wxyz
pose = lcmdrc.position_3d_t()
pose.translation = trans
pose.rotation = quat
return pose
def getPoseLCMFromXYZRPY(xyz, rpy):
wxyz = botpy.roll_pitch_yaw_to_quat(rpy)
trans = lcmdrc.vector_3d_t()
trans.x, trans.y, trans.z = xyz
quat = lcmdrc.quaternion_t()
quat.w, quat.x, quat.y, quat.z = wxyz
pose = lcmdrc.position_3d_t()
pose.translation = trans
pose.rotation = quat
return pose
def getPoseLCMFromXYZRPY(xyz, rpy):
wxyz = transformUtils.rollPitchYawToQuaternion(rpy)
trans = lcmdrc.vector_3d_t()
trans.x, trans.y, trans.z = xyz
quat = lcmdrc.quaternion_t()
quat.w, quat.x, quat.y, quat.z = wxyz
pose = lcmdrc.position_3d_t()
pose.translation = trans
pose.rotation = quat
return pose
def getPoseLCMFromXYZRPY(xyz, rpy):
wxyz = botpy.roll_pitch_yaw_to_quat(rpy)
trans = lcmdrc.vector_3d_t()
trans.x, trans.y, trans.z = xyz
quat = lcmdrc.quaternion_t()
quat.w, quat.x, quat.y, quat.z = wxyz
pose = lcmdrc.position_3d_t()
pose.translation = trans
pose.rotation = quat
return pose
def positionMessageFromFrame(transform):
'''
Given a vtkTransform, returns an lcmdrc.position_t message
'''
pos, wxyz = transformUtils.poseFromTransform(transform)
trans = lcmdrc.vector_3d_t()
trans.x, trans.y, trans.z = pos
quat = lcmdrc.quaternion_t()
quat.w, quat.x, quat.y, quat.z = wxyz
pose = lcmdrc.position_3d_t()
pose.translation = trans
pose.rotation = quat
return pose
def positionMessageFromFrame(transform):
'''
Given a vtkTransform, returns an lcmdrc.position_t message
'''
pos, wxyz = poseFromTransform(transform)
trans = lcmdrc.vector_3d_t()
trans.x, trans.y, trans.z = pos
quat = lcmdrc.quaternion_t()
quat.w, quat.x, quat.y, quat.z = wxyz
pose = lcmdrc.position_3d_t()
pose.translation = trans
pose.rotation = quat
return pose