def __init__(self):
# initialize atlas joint names
self._atlasJointNames = [
'atlas::back_lbz', 'atlas::back_mby', 'atlas::back_ubx',
'atlas::neck_ay', 'atlas::l_leg_uhz', 'atlas::l_leg_mhx',
'atlas::l_leg_lhy', 'atlas::l_leg_kny', 'atlas::l_leg_uay',
'atlas::l_leg_lax', 'atlas::r_leg_uhz', 'atlas::r_leg_mhx',
'atlas::r_leg_lhy', 'atlas::r_leg_kny', 'atlas::r_leg_uay',
'atlas::r_leg_lax', 'atlas::l_arm_usy', 'atlas::l_arm_shx',
'atlas::l_arm_ely', 'atlas::l_arm_elx', 'atlas::l_arm_uwy',
'atlas::l_arm_mwx', 'atlas::r_arm_usy', 'atlas::r_arm_shx',
'atlas::r_arm_ely', 'atlas::r_arm_elx', 'atlas::r_arm_uwy',
'atlas::r_arm_mwx'
]
# initialize new constants
self._kp_position = [
800, 4000, 2000, 20, 1000, 1200, 1100, 1000, 1200, 600, 1000, 1200,
1100, 1000, 1200, 600, 2000, 1000, 30, 700, 50, 100, 2000, 1000,
30, 700, 50, 100
]
self._kd_position = [
0, 2, 1, 1, 0, 0, 0, 10, 0, 0, 0, 0, 0, 10, 0, 0, 3, 10, 2, 3, 0.1,
0.2, 3, 10, 2, 3, 0.1, 0.2
]
# joint states subscriber
self._currentJointState = JointState()
# initialize command
self._command = AtlasCommand()
#self._command.name = list(self._atlasJointNames)
n = len(self._atlasJointNames)
self._command.velocity = zeros(n)
self._command.effort = zeros(n)
self._command.kp_position = zeros(n)
self._command.ki_position = zeros(n)
self._command.kd_position = zeros(n)
self._command.kp_velocity = zeros(n)
self._command.i_effort_min = zeros(n)
self._command.i_effort_max = zeros(n)
# Set k_effort to 255 to indicate that we want PID control of each joint
self._command.k_effort = [255] * n
for i in xrange(len(self._atlasJointNames)):
name = self._atlasJointNames[i]
self._command.kp_position[i] = self._kp_position[i]
self._command.ki_position[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/i')
self._command.kd_position[i] = self._kd_position[i]
self._command.i_effort_max[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/i_clamp')
self._command.i_effort_min[i] = -self._command.i_effort_max[i]
rospy.Subscriber("/atlas/joint_states", JointState,
self._jointStatesCallback)
rospy.Subscriber("/atlas/imu", Imu, self._OrientationCallback)
self._pub = rospy.Publisher('/atlas/atlas_command', AtlasCommand)
rospy.sleep(0.5)
def go():
pub = rospy.Publisher('atlas/atlas_command', AtlasCommand)
while not rospy.is_shutdown():
ac = AtlasCommand()
# Wiggle it, just a little bit
pos = 0.1 * math.sin(time.time())
for i in range(0, NUM_JOINTS):
ac.position.append(pos)
ac.velocity.append(0.0)
ac.effort.append(0.0)
pub.publish(ac)
time.sleep(0.1)
def __init__(self):
self.Joint_dict = {AtlasCommand_msg_handler.atlasJointNames[i]:i for i in xrange(len(AtlasCommand_msg_handler.atlasJointNames))}
self.inv_dict = dict(zip(self.Joint_dict.values(), self.Joint_dict.keys()))
if rospy.get_name() == '/unnamed':
rospy.init_node('AtlasCommand_msg_handler',anonymous=True)
self._ac_pub = rospy.Publisher('atlas/atlas_command', AtlasCommand)
self._command = AtlasCommand()
self._joint_names = list(self.atlasJointNames)
self._command.k_effort = [255] * len(self._joint_names)
self.set_default_gains_from_param()
self.reset_command()
def _proceed_with_walking(self):
message = AtlasCommand()
message.position = self._current_position
# position_list = list(self._current_position)
# if self._global_record_counter == len(self._position_record)-1:
# self._global_record_counter = 0
# else:
# self._global_record_counter += 1
# for i in xrange(5,16):
# position_list[i] = self._position_record[
# self._global_record_counter][i]
# message.position = tuple(position_list)
message.k_effort = [255] * 28
self._atlas_command_publisher.publish(message)
time.sleep(0.02)
def __init__(self, robot_name, robot_joints):
self.RobotName = robot_name
self.JointNames = robot_joints
self.Joint_dict = {
self.JointNames[i]: i
for i in xrange(len(self.JointNames))
}
self.inv_dict = dict(
zip(self.Joint_dict.values(), self.Joint_dict.keys()))
if rospy.get_name() == '/unnamed':
rospy.init_node('JointCommands_msg_handler', anonymous=True)
self._compub = rospy.Publisher('/' + self.RobotName + '/atlas_command',
AtlasCommand)
self._command = AtlasCommand()
self.set_default_gains_from_param()
self.reset_command()
def demo(self):
# Step 1: Go to stand-prep pose under user control
stand_prep_msg = AtlasCommand()
# Always insert current time
stand_prep_msg.header.stamp = rospy.Time.now()
# Assign some position and gain values that will get us there.
stand_prep_msg.position = [
2.438504816382192e-05, 0.0015186156379058957,
9.983908967114985e-06, -0.0010675729718059301,
-0.0003740221436601132, 0.06201673671603203, -0.2333149015903473,
0.5181407332420349, -0.27610817551612854, -0.062101610004901886,
0.00035181696875952184, -0.06218484416604042, -0.2332201600074768,
0.51811283826828, -0.2762000858783722, 0.06211360543966293,
0.29983898997306824, -1.303462266921997, 2.0007927417755127,
0.49823325872421265, 0.0003098883025813848, -0.0044272784143686295,
0.29982614517211914, 1.3034454584121704, 2.000779867172241,
-0.498238742351532, 0.0003156556049361825, 0.004448802210390568
]
stand_prep_msg.velocity = [0.0] * self.NUM_JOINTS
stand_prep_msg.effort = [0.0] * self.NUM_JOINTS
stand_prep_msg.k_effort = [255] * self.NUM_JOINTS
# Publish and give time to take effect
print('[USER] Going to stand prep position...')
self.ac_pub.publish(stand_prep_msg)
time.sleep(2.0)
# Step 2: Request BDI stand mode
stand_msg = AtlasSimInterfaceCommand()
# Always insert current time
stand_msg.header.stamp = rospy.Time.now()
# Tell it to stand
stand_msg.behavior = stand_msg.STAND_PREP
# Set k_effort = [255] to indicate that we still want all joints under user
# control. The stand behavior needs a few iterations before it start
# outputting force values.
stand_msg.k_effort = [255] * self.NUM_JOINTS
# Publish and give time to take effect
print('[USER] Warming up BDI stand...')
self.asic_pub.publish(stand_msg)
time.sleep(1.0)
# Now switch to stand
stand_msg.behavior = stand_msg.STAND
# Set k_effort = [0] to indicate that we want all joints under BDI control
stand_msg.k_effort = [0] * self.NUM_JOINTS
# Publish and give time to take effect
print('[BDI] Standing...')
self.asic_pub.publish(stand_msg)
time.sleep(2.0)
# Step 3: Move the arms and head a little (not too much; don't want to fall
# over)
slight_movement_msg = AtlasCommand()
# Always insert current time
slight_movement_msg.header.stamp = rospy.Time.now()
# Start with 0.0 and set values for the joints that we want to control
slight_movement_msg.position = [0.0] * self.NUM_JOINTS
slight_movement_msg.position[AtlasState.neck_ry] = -0.1
slight_movement_msg.position[AtlasState.l_arm_ely] = 2.1
slight_movement_msg.position[AtlasState.l_arm_wrx] = -0.1
slight_movement_msg.position[AtlasState.r_arm_ely] = 2.1
slight_movement_msg.position[AtlasState.r_arm_wrx] = -0.1
slight_movement_msg.velocity = [0.0] * self.NUM_JOINTS
slight_movement_msg.effort = [0.0] * self.NUM_JOINTS
slight_movement_msg.kp_position = [
20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0,
300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0,
200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0,
100.0
]
slight_movement_msg.ki_position = [0.0] * self.NUM_JOINTS
slight_movement_msg.kd_position = [0.0] * self.NUM_JOINTS
# Bump up kp_velocity to reduce the jerkiness of the transition
stand_prep_msg.kp_velocity = [50.0] * self.NUM_JOINTS
slight_movement_msg.i_effort_min = [0.0] * self.NUM_JOINTS
slight_movement_msg.i_effort_max = [0.0] * self.NUM_JOINTS
# Set k_effort = [1] for the joints that we want to control.
# BDI has control of the other joints
slight_movement_msg.k_effort = [0] * self.NUM_JOINTS
slight_movement_msg.k_effort[AtlasState.neck_ry] = 255
slight_movement_msg.k_effort[AtlasState.l_arm_ely] = 255
slight_movement_msg.k_effort[AtlasState.l_arm_wrx] = 255
slight_movement_msg.k_effort[AtlasState.r_arm_ely] = 255
slight_movement_msg.k_effort[AtlasState.r_arm_wrx] = 255
# Publish and give time to take effect
print('[USER/BDI] Command neck and arms...')
self.ac_pub.publish(slight_movement_msg)
time.sleep(2.0)
# Step 4: Request BDI walk
walk_msg = AtlasSimInterfaceCommand()
# Always insert current time
walk_msg.header.stamp = rospy.Time.now()
# Tell it to walk
walk_msg.behavior = walk_msg.WALK
walk_msg.walk_params.use_demo_walk = False
# Fill in some steps
for i in range(4):
step_data = AtlasBehaviorStepData()
# Steps are indexed starting at 1
step_data.step_index = i + 1
# 0 = left, 1 = right
step_data.foot_index = i % 2
# 0.3 is a good number
step_data.swing_height = 0.3
# 0.63 is a good number
step_data.duration = 0.63
# We'll specify desired foot poses in ego-centric frame then
# transform them into the robot's world frame.
# Match feet so that we end with them together
step_data.pose.position.x = (1 + i / 2) * 0.25
# Step 0.15m to either side of center, alternating with feet
step_data.pose.position.y = 0.15 if (i % 2 == 0) else -0.15
step_data.pose.position.z = 0.0
# Point those feet straight ahead
step_data.pose.orientation.x = 0.0
step_data.pose.orientation.y = 0.0
step_data.pose.orientation.z = 0.0
step_data.pose.orientation.w = 1.0
# Transform this foot pose according to robot's
# current estimated pose in the world, which is a combination of IMU
# and internal position estimation.
# http://www.ros.org/wiki/kdl/Tutorials/Frame%20transformations%20%28Python%29
f1 = posemath.fromMsg(step_data.pose)
f2 = PyKDL.Frame(
PyKDL.Rotation.Quaternion(self.imu_msg.orientation.x,
self.imu_msg.orientation.y,
self.imu_msg.orientation.z,
self.imu_msg.orientation.w),
PyKDL.Vector(self.asis_msg.pos_est.position.x,
self.asis_msg.pos_est.position.y,
self.asis_msg.pos_est.position.z))
f = f2 * f1
step_data.pose = posemath.toMsg(f)
walk_msg.walk_params.step_queue[i] = step_data
# Use the same k_effort from the last step, to retain user control over some
# joints. BDI has control of the other joints.
walk_msg.k_effort = slight_movement_msg.k_effort
# Publish and give time to take effect
print('[USER/BDI] Walking...')
self.asic_pub.publish(walk_msg)
time.sleep(6.0)
# Step 5: Go back to home pose under user control
home_msg = AtlasCommand()
# Always insert current time
home_msg.header.stamp = rospy.Time.now()
# Assign some position and gain values that will get us there.
home_msg.position = [0.0] * self.NUM_JOINTS
home_msg.velocity = [0.0] * self.NUM_JOINTS
home_msg.effort = [0.0] * self.NUM_JOINTS
home_msg.kp_position = [
20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0,
300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0,
200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0,
100.0
]
home_msg.ki_position = [0.0] * self.NUM_JOINTS
home_msg.kd_position = [0.0] * self.NUM_JOINTS
# Bump up kp_velocity to reduce the jerkiness of the transition
home_msg.kp_velocity = [50.0] * self.NUM_JOINTS
home_msg.i_effort_min = [0.0] * self.NUM_JOINTS
home_msg.i_effort_max = [0.0] * self.NUM_JOINTS
# Set k_effort = [1] to indicate that we want all joints under user control
home_msg.k_effort = [255] * self.NUM_JOINTS
# Publish and give time to take effect
print('[USER] Going to home position...')
self.ac_pub.publish(home_msg)
time.sleep(2.0)
#!/usr/bin/env python
import roslib; roslib.load_manifest('atlas_teleop')
import rospy, sys, yaml
from osrf_msgs.msg import JointCommands
from atlas_msgs.msg import AtlasCommand
from sensor_msgs.msg import Joy
g_ac_pub = None
g_jc_lh_pub = None
g_jc_rh_pub = None
g_jc = AtlasCommand()
g_jc_rh = JointCommands()
g_jc_lh = JointCommands()
g_vec = [ ]
g_knobs = "unknown"
g_vec_lh = [ ]
g_vec_rh = [ ]
# need to figure out a reasonable way to blend grasps composed of different
# origins... for now, it will ignore the 'origin' data and just use zero...
g_grasps = { 'cyl': { 'origin':[0] * 12,
'grasp': [ 0, 1.5, 1.7, 0, 1.5, 1.7, 0, 1.5, 1.7, -0.2, 0.8, 1.7]},
'sph': { 'origin':[ 0.0, 0, 0, 0.1, 0, 0, 0.0, 0, 0, 0, 0, 0],
'grasp': [ -1.0, 1.4, 1.4, 0.0, 1.4, 1.4, 1.0, 1.4, 1.4, 0, 0.7, 0.7]},
'par': { 'origin':[ 0.0, 0.0, 0.0, 0, -1.4, -1.4, 0.0, -1.4, -1.4, 0.5, 0.0, 0.0],
'grasp': [ 0.0, 1.4, 1.2, 0, -1.4, -1.4, 0.0, -1.4, -1.4, 0.5, 0.7, 0.7]} }
def joy_cb(msg):
global g_ac_pub, g_jc
g_jc.position = [0] * 28
g_jc_lh.position = [0] * 12
g_jc_rh.position = [0] * 12
for x in xrange(0, 28): # start at origin