def main():
try:
rospy.init_node('avalos_limb_py')
limb = Limb()
print "Joint Name:"
print limb.joint_names()
print "\n" + "Joint Angles:"
print limb.joint_angles()
print "\n" + "Joint Velocities:"
print limb.joint_velocities()
print "\n" + "Endpoint Pose:"
print limb.endpoint_pose()
print "\n" + "Endpoint Velocity:"
print limb.endpoint_velocity()
except rospy.ROSInterruptException:
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
def goToPose(self):
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions(max_linear_speed=0.1,
max_linear_accel=0.1,
max_rotational_speed=0.1,
max_rotational_accel=0.1,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_names = limb.joint_names()
waypoint.set_joint_angles(self.ikResults.values(), "left_hand",
joint_names)
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=30.0)
if result is None:
rospy.logerr('Trajectory Failed')
return
if result.result:
rospy.loginfo('Trajectory Success')
else:
rospy.logerr('Trajectory failed with error %s', result.errorId)
class KBEnv(object):
def __init__(self, path):
rospy.init_node('sawyer_arm_cntrl')
self.load_inits(path+'/pg_init.yaml')
self.limb = Limb()
self.all_jnts = copy.copy(self.limb.joint_names())
self.limb.set_joint_position_speed(0.2)
def load_inits(self, path):
stream = open(path, "r")
config = yaml.load(stream)
stream.close()
# these indices and names have to be in ascending order
self.jnt_indices = config['jnt_indices']
self.cmd_names = config['cmd_names']
self.init_pos = config['initial-position']
def chng_jnt_state(self, values, mode, iterations=20):
# Command Sawyer's joints to angles or velocity
robot = URDF.from_parameter_server()
kdl_kin_r = KDLKinematics(robot, 'base',
'right_gripper_r_finger_tip')
kdl_kin_l = KDLKinematics(robot, 'base',
'right_gripper_l_finger_tip')
q_jnt_angles = np.zeros(8)
q_jnt_angles[:7] = copy.copy(values)
q_jnt_angles[7] = 0.0
pose_r = kdl_kin_r.forward(q_jnt_angles)
pose_l = kdl_kin_l.forward(q_jnt_angles)
pose = 0.5*(pose_l + pose_r)
x, y, z = transformations.translation_from_matrix(pose)[:3]
print("goal cartesian: ", x, y, z)
timeout = 30.0
if mode == 4:
positions = dict()
i = 0
for jnt_name in self.all_jnts:
positions[jnt_name] = values[i]
i += 1
self.limb.move_to_joint_positions(positions, timeout)
else:
velocities = dict()
i = 0
for name in self.cmd_names:
velocities[name] = values[i]
i += 1
for _ in range(iterations):
self.limb.set_joint_velocities(velocities)
time.sleep(0.5)
def moveRoboticArm(position, orientation, linear_speed, linear_accel):
"""
Move the robot arm to the specified configuration given a positionX, positionY, positionZ, quaternion array and max linear speed.
"""
try:
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions(max_linear_speed=linear_speed,
max_linear_accel=linear_accel)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_names = limb.joint_names()
endpoint_state = limb.tip_state('right_hand')
pose = endpoint_state.pose
if position is not None and len(position) == 3:
pose.position.x = position[0]
pose.position.y = position[1]
pose.position.z = position[2]
if orientation is not None and len(orientation) == 4:
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[3]
poseStamped = PoseStamped()
poseStamped.pose = pose
joint_angles = limb.joint_ordered_angles()
waypoint.set_cartesian_pose(poseStamped, 'right_hand', joint_angles)
rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string())
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory()
if result is None:
rospy.logerr('Trajectory FAILED to send')
return
if result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!')
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
print("Something went wrong")
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
def initialize_jnts(self):
print("Initializing joints...")
positions = dict()
limb = Limb()
calib_angles = [0.27, -3.27, 3.04, -1.60, -0.38, -1.66, 0.004]
all_jnts = copy.copy(limb.joint_names())
limb.set_joint_position_speed(0.2)
positions['head_pan'] = 0.0
enum_iter = enumerate(all_jnts, start=0)
for i, jnt_name in enum_iter:
positions[jnt_name] = calib_angles[i]
limb.move_to_joint_positions(positions)
print("Done Initializing joints!!")
def goto_cartesian(self, x, y, z):
print("goto_cartesian called with x={}, y={}, z={}".format(x, y, z))
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions()
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_names = limb.joint_names()
endpoint_state = limb.tip_state('right_hand')
if endpoint_state is None:
print('Endpoint state not found')
return self.MOVE_ERROR
pose = endpoint_state.pose
pose.position.x = x
pose.position.y = y
pose.position.z = z
pose.orientation.x = self.orientation_x
pose.orientation.y = self.orientation_y
pose.orientation.z = self.orientation_z
pose.orientation.w = self.orientation_w
poseStamped = PoseStamped()
poseStamped.pose = pose
waypoint.set_cartesian_pose(poseStamped, 'right_hand', [])
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory()
if result is None:
print("Trajectory FAILED to send")
return self.MOVE_ERROR
if result.result:
print('Motion controller successfully finished the trajectory!')
self.cur_x = x
self.cur_y = y
self.cur_z = z
return self.MOVE_SUCCESS
else:
print('Motion controller failed to complete the trajectory %s',
result.errorId)
return self.MOVE_ERROR
def gripper_pose(x, y, z):
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions()
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_names = limb.joint_names()
endpoint_state = limb.tip_state('right_hand')
if endpoint_state is None:
print('Endpoint state not found')
return False
pose = endpoint_state.pose
pose.position.x = x
pose.position.y = y
pose.position.z = z
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
poseStamped = PoseStamped()
poseStamped.pose = pose
waypoint.set_cartesian_pose(poseStamped, 'right_hand', [])
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory()
if result is None:
print("Trajectory FAILED to send")
return False
if result.result:
return True
else:
print('Motion controller failed to complete the trajectory %s',
result.errorId)
return False
def move2cartesian(position=None, orientation=None, relative_pose=None, in_tip_frame=False, joint_angles=[],
tip_name='right_hand', linear_speed=0.6, linear_accel=0.6, rotational_speed=1.57,
rotational_accel=1.57, timeout=None, neutral=False):
"""
Move the robot arm to the specified configuration.
Call using:
$ rosrun intera_examples go_to_cartesian_pose.py [arguments: see below]
-p 0.4 -0.3 0.18 -o 0.0 1.0 0.0 0.0 -t right_hand
--> Go to position: x=0.4, y=-0.3, z=0.18 meters
--> with quaternion orientation (0, 1, 0, 0) and tip name right_hand
--> The current position or orientation will be used if only one is provided.
-q 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0
--> Go to joint angles: 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 using default settings
--> If a Cartesian pose is not provided, Forward kinematics will be used
--> If a Cartesian pose is provided, the joint angles will be used to bias the nullspace
-R 0.01 0.02 0.03 0.1 0.2 0.3 -T
-> Jog arm with Relative Pose (in tip frame)
-> x=0.01, y=0.02, z=0.03 meters, roll=0.1, pitch=0.2, yaw=0.3 radians
-> The fixed position and orientation paramters will be ignored if provided
"""
try:
#rospy.init_node('go_to_cartesian_pose_py')
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options = traj_options, limb = limb)
wpt_opts = MotionWaypointOptions(max_linear_speed=linear_speed,
max_linear_accel=linear_accel,
max_rotational_speed=rotational_speed,
max_rotational_accel=rotational_accel,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
joint_names = limb.joint_names()
if joint_angles and len(joint_angles) != len(joint_names):
rospy.logerr('len(joint_angles) does not match len(joint_names!)')
return None
if neutral == True:
limb.move_to_neutral()
else:
if (position is None and orientation is None and relative_pose is None):
if joint_angles:
# does Forward Kinematics
waypoint.set_joint_angles(joint_angles, tip_name, joint_names)
else:
rospy.loginfo("No Cartesian pose or joint angles given. Using default")
waypoint.set_joint_angles(joint_angles=None, active_endpoint=tip_name)
else:
endpoint_state = limb.tip_state(tip_name)
if endpoint_state is None:
rospy.logerr('Endpoint state not found with tip name %s', tip_name)
return None
pose = endpoint_state.pose
if relative_pose is not None:
if len(relative_pose) != 6:
rospy.logerr('Relative pose needs to have 6 elements (x,y,z,roll,pitch,yaw)')
return None
# create kdl frame from relative pose
rot = PyKDL.Rotation.RPY(relative_pose[3],
relative_pose[4],
relative_pose[5])
trans = PyKDL.Vector(relative_pose[0],
relative_pose[1],
relative_pose[2])
f2 = PyKDL.Frame(rot, trans)
# and convert the result back to a pose message
if in_tip_frame:
# end effector frame
pose = posemath.toMsg(posemath.fromMsg(pose) * f2)
else:
# base frame
pose = posemath.toMsg(f2 * posemath.fromMsg(pose))
else:
if position is not None and len(position) == 3:
pose.position.x = position[0]
pose.position.y = position[1]
pose.position.z = position[2]
if orientation is not None and len(orientation) == 4:
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[3]
poseStamped = PoseStamped()
poseStamped.pose = pose
if not joint_angles:
# using current joint angles for nullspace bais if not provided
joint_angles = limb.joint_ordered_angles()
waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles)
else:
waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles)
rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string())
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=timeout)
if result is None:
rospy.logerr('Trajectory FAILED to send')
return
if result.result:
rospy.loginfo('Motion controller successfully finished the trajectory!')
else:
rospy.logerr('Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.')
def cartesian_pose(args):
"""
Move the robot arm to the specified configuration.
Call using:
$ rosrun intera_examples go_to_cartesian_pose.py [arguments: see below]
-p 0.4 -0.3 0.18 -o 0.0 1.0 0.0 0.0 -t right_hand
--> Go to position: x=0.4, y=-0.3, z=0.18 meters
--> with quaternion orientation (0, 1, 0, 0) and tip name right_hand
--> The current position or orientation will be used if only one is provided.
-q 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0
--> Go to joint angles: 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 using default settings
--> If a Cartesian pose is not provided, Forward kinematics will be used
--> If a Cartesian pose is provided, the joint angles will be used to bias the nullspace
-R 0.01 0.02 0.03 0.1 0.2 0.3 -T
-> Jog arm with Relative Pose (in tip frame)
-> x=0.01, y=0.02, z=0.03 meters, roll=0.1, pitch=0.2, yaw=0.3 radians
-> The fixed position and orientation paramters will be ignored if provided
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description="cartesian_pose.__doc__")
parser.add_argument(
"-p", "--position", type=float,
nargs='+',
help="Desired end position: X, Y, Z")
parser.add_argument(
"-o", "--orientation", type=float,
nargs='+',
help="Orientation as a quaternion (x, y, z, w)")
parser.add_argument(
"-R", "--relative_pose", type=float,
nargs='+',
help="Jog pose by a relative amount in the base frame: X, Y, Z, roll, pitch, yaw")
parser.add_argument(
"-T", "--in_tip_frame", action='store_true',
help="For relative jogs, job in tip frame (default is base frame)")
parser.add_argument(
"-q", "--joint_angles", type=float,
nargs='+', default=[],
help="A list of joint angles, one for each of the 7 joints, J0...J6")
parser.add_argument(
"-t", "--tip_name", default='right_hand',
help="The tip name used by the Cartesian pose")
parser.add_argument(
"--linear_speed", type=float, default=0.6,
help="The max linear speed of the endpoint (m/s)")
parser.add_argument(
"--linear_accel", type=float, default=0.6,
help="The max linear acceleration of the endpoint (m/s/s)")
parser.add_argument(
"--rotational_speed", type=float, default=1.57,
help="The max rotational speed of the endpoint (rad/s)")
parser.add_argument(
"--rotational_accel", type=float, default=1.57,
help="The max rotational acceleration of the endpoint (rad/s/s)")
parser.add_argument(
"--timeout", type=float, default=None,
help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.")
"""
try:
#rospy.init_node('go_to_cartesian_pose_py')
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions(
max_linear_speed=args.linear_speed,
max_linear_accel=args.linear_accel,
max_rotational_speed=args.rotational_speed,
max_rotational_accel=args.rotational_accel,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_names = limb.joint_names()
if args.joint_angles and len(args.joint_angles) != len(joint_names):
rospy.logerr('len(joint_angles) does not match len(joint_names!)')
return None
if (args.position is None and args.orientation is None
and args.relative_pose is None):
if args.joint_angles:
# does Forward Kinematics
waypoint.set_joint_angles(args.joint_angles, args.tip_name,
joint_names)
else:
rospy.loginfo(
"No Cartesian pose or joint angles given. Using default")
waypoint.set_joint_angles(joint_angles=None,
active_endpoint=args.tip_name)
else:
endpoint_state = limb.tip_state(args.tip_name)
if endpoint_state is None:
rospy.logerr('Endpoint state not found with tip name %s',
args.tip_name)
return None
pose = endpoint_state.pose
if args.relative_pose is not None:
if len(args.relative_pose) != 6:
rospy.logerr(
'Relative pose needs to have 6 elements (x,y,z,roll,pitch,yaw)'
)
return None
# create kdl frame from relative pose
rot = PyKDL.Rotation.RPY(args.relative_pose[3],
args.relative_pose[4],
args.relative_pose[5])
trans = PyKDL.Vector(args.relative_pose[0],
args.relative_pose[1],
args.relative_pose[2])
f2 = PyKDL.Frame(rot, trans)
# and convert the result back to a pose message
if args.in_tip_frame:
# end effector frame
pose = posemath.toMsg(posemath.fromMsg(pose) * f2)
else:
# base frame
pose = posemath.toMsg(f2 * posemath.fromMsg(pose))
else:
if args.position is not None and len(args.position) == 3:
pose.position.x = args.position[0]
pose.position.y = args.position[1]
pose.position.z = args.position[2]
if args.orientation is not None and len(args.orientation) == 4:
pose.orientation.x = args.orientation[0]
pose.orientation.y = args.orientation[1]
pose.orientation.z = args.orientation[2]
pose.orientation.w = args.orientation[3]
poseStamped = PoseStamped()
poseStamped.pose = pose
waypoint.set_cartesian_pose(poseStamped, args.tip_name,
args.joint_angles)
rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string())
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=args.timeout)
if result is None:
rospy.logerr('Trajectory FAILED to send')
return
if result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!')
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
def go_to_cartesian(position=None,
orientation=None,
joint_angles=None,
linear_speed=0.1,
rotational_speed=0.57,
linear_accel=0.3,
tip_name='right_hand',
relative_pose=None,
rotational_accel=0.57,
timeout=None):
if not rospy.is_shutdown():
try:
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions(
max_linear_speed=linear_speed,
max_linear_accel=linear_accel,
max_rotational_speed=rotational_speed,
max_rotational_accel=rotational_accel,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_names = limb.joint_names()
if joint_angles and len(joint_angles) != len(joint_names):
rospy.logerr(
'len(joint_angles) does not match len(joint_names!)')
return None
if (position is None and orientation is None
and relative_pose is None):
if joint_angles:
# does Forward Kinematics
waypoint.set_joint_angles(joint_angles, tip_name,
joint_names)
else:
rospy.loginfo(
"No Cartesian pose or joint angles given. Using default"
)
waypoint.set_joint_angles(joint_angles=None,
active_endpoint=tip_name)
else:
endpoint_state = limb.tip_state(tip_name)
if endpoint_state is None:
rospy.logerr('Endpoint state not found with tip name %s',
tip_name)
return None
pose = endpoint_state.pose
if relative_pose is not None:
if len(relative_pose) != 6:
rospy.logerr(
'Relative pose needs to have 6 elements (x,y,z,roll,pitch,yaw)'
)
return None
# create kdl frame from relative pose
rot = PyKDL.Rotation.RPY(relative_pose[3],
relative_pose[4],
relative_pose[5])
trans = PyKDL.Vector(relative_pose[0], relative_pose[1],
relative_pose[2])
f2 = PyKDL.Frame(rot, trans)
# and convert the result back to a pose message
if in_tip_frame:
# end effector frame
pose = posemath.toMsg(posemath.fromMsg(pose) * f2)
else:
# base frame
pose = posemath.toMsg(f2 * posemath.fromMsg(pose))
else:
if position is not None:
pose.position.x = position.x
pose.position.y = position.y
pose.position.z = position.z
if orientation is not None:
pose.orientation.x = orientation.x
pose.orientation.y = orientation.y
pose.orientation.z = orientation.z
pose.orientation.w = orientation.w
poseStamped = PoseStamped()
poseStamped.pose = pose
waypoint.set_cartesian_pose(poseStamped, tip_name,
joint_angles)
#rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string())
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=timeout)
if result is None:
return
if result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!')
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
class CircularMotion():
def __init__(self):
self.mover = move_to_point.MoveToPoint()
self.retrieve_height = -0.015
self._limb = Limb()
self.traj_options = TrajectoryOptions()
self.traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
self.traj = MotionTrajectory(trajectory_options=self.traj_options,
limb=self._limb)
self.wpt_options = MotionWaypointOptions(max_linear_speed=0.4,
max_linear_accel=0.4,
max_rotational_speed=1.57,
max_rotational_accel=1.57,
max_joint_speed_ratio=1.0)
self.waypoint = MotionWaypoint(options=self.wpt_options.to_msg(),
limb=self._limb)
self.joint_names = self._limb.joint_names()
endpoint_state = self._limb.tip_state('right_hand')
self.pose = endpoint_state.pose
def to_quaternion(self, roll, pitch, yaw):
cy = math.cos(0.5 * yaw)
sy = math.sin(0.5 * yaw)
cr = math.cos(0.5 * roll)
sr = math.sin(0.5 * roll)
cp = math.cos(0.5 * pitch)
sp = math.sin(0.5 * pitch)
qx = cy * sr * cp - sy * cr * sp
qy = cy * cr * sp + sy * sr * cp
qz = sy * cr * cp - cy * sr * sp
qw = cy * cr * cp + sy * sr * sp
orientation = [qx, qy, qz, qw]
return orientation
def displacement(self, time):
disp = 0.1667 * math.pi * math.sin(time)
return disp
def move(self, cur_pos):
curr_roll = 1.1667 * math.pi
curr_pitch = 0
curr_yaw = 0
self.mover.move(cur_pos[0], cur_pos[1], self.retrieve_height,
cur_pos[3], cur_pos[4], cur_pos[5], cur_pos[6])
rospy.sleep(1)
time = 0
while time < 2 * math.pi:
end = self.to_quaternion(curr_roll, curr_pitch, curr_yaw)
self.pose.position.x = cur_pos[0]
self.pose.position.y = cur_pos[1]
self.pose.position.z = self.retrieve_height
self.pose.orientation.x = end[0]
self.pose.orientation.y = end[1]
self.pose.orientation.z = end[2]
self.pose.orientation.w = end[3]
if time < 0.5 * math.pi:
curr_roll = 1.1667 * math.pi - self.displacement(time)
elif time < math.pi:
curr_roll = 0.833 * math.pi + self.displacement(time)
elif time < 1.5 * math.pi:
curr_roll = 0.833 * math.pi - self.displacement(time)
else:
curr_roll = 1.1667 * math.pi + self.displacement(time)
curr_pitch = self.displacement(time)
time = time + 0.1
poseStamped = PoseStamped()
poseStamped.pose = self.pose
self.waypoint.set_cartesian_pose(poseStamped, 'right_hand', [])
self.traj.append_waypoint(self.waypoint.to_msg())
result = self.traj.send_trajectory(timeout=None)
class Sawyer_Head:
def get_neutral_base_joint_angle(self):
angle=rospy.get_param("named_poses/right/poses/shipping")[1]
return angle
def __init__(self):
self._done = False
self._head = Head()
self._limb=Limb()
self._robot_state = RobotEnable(CHECK_VERSION)
self._init_state = self._robot_state.state().enabled
self._robot_state.enable()
self._display = HeadDisplay()
face_forward_service = rospy.Service('head/face_forward', FaceForward, self.face_forward)
rotate_head_service= rospy.Service('head/pan_to_angle', PanToAngle, self.pan_to_angle)
# arrow_key_control_servie = rospy.Service('head/arrow_key_control', PanToAngle, self.turn_with_arrow_keys)
# rospy.Subscriber("robot/head/head_state", HeadState, self.straighten)
# def straighten(self,headState):
# try:
# self._head.set_pan(math.pi/2,speed=1.0,timeout=1.0,active_cancellation=True)
# except OSError, e:
# print(e)
def get_base_joint_angle(self):
base_joint_name=self._limb.joint_names()[0]
return self._limb.joint_angle(base_joint_name)
def set_angle(self,angle):
'''
Sets head to the desired angle
0 - left
pi/2 - forward
pi - right
'''
self._head.set_pan(angle)
def pan_to_angle(self, req):
try:
angle=req.theta
self._head.set_pan(angle)
return PanToAngleResponse(True)
except:
return PanToAngleResponse(False)
def face_forward(self, object):
'''
Sets head facing forward regardless of body position
'''
try:
base_angle= self.get_base_joint_angle()
head_angle= self._head.pan()
angle=-1*base_angle+ math.pi/2
self.set_angle(angle)
return FaceForwardResponse(True)
except:
return FaceForwardResponse(False)
def turn_head(self, req):
try:
head_angle=self._head.pan()
if req.left:
self.set_angle(head_angle+math.radians(3))
else:
self.set_angle(head_angle-math.radians(3))
except:
rospy.logwarn("can't turn head any further")
def display(self, face):
'''
display a face on sawyer
'''
pass
#!/usr/bin/env python
import rospy
import numpy as np
import itertools
from intera_interface import Limb
if __name__ == '__main__':
rospy.init_node('fix_alan')
limb = Limb('right')
while not rospy.is_shutdown():
limb.set_joint_positions(dict(itertools.izip(limb.joint_names(), np.zeros(len(limb.joint_names())))))
rospy.spin()
class Env(object):
def __init__(self, path, train_mode=True):
self.train_mode = train_mode
self.obs_lock = threading.Lock()
# path where the python script for agent and env reside
self.path = path
string = "sawyer_ros"
rospy.init_node(string + "_environment")
self.logp = None
if self.train_mode:
self.logpath = "AC" + '_' + time.strftime("%d-%m-%Y_%H-%M")
self.logpath = os.path.join(path + '/data', self.logpath)
if not (os.path.exists(self.logpath)):
# we should never have to create dir, as agent already done it
os.makedirs(self.logpath)
logfile = os.path.join(self.logpath, "ros_env.log")
self.logp = open(logfile, "w")
# Goal does not move and is specified by three points (for pose reasons)
self.goal_pos_x1 = None
self.goal_pos_y1 = None
self.goal_pos_z1 = None
self.goal_pos_x2 = None
self.goal_pos_y2 = None
self.goal_pos_z2 = None
self.goal_pos_x3 = None
self.goal_pos_y3 = None
self.goal_pos_z3 = None
self._load_inits(path)
self.cur_obs = np.zeros(self.obs_dim)
self.cur_act = np.zeros(self.act_dim)
self.cur_reward = None
self.goal_cntr = 0
self.limb = Limb()
self.all_jnts = copy.copy(self.limb.joint_names())
self.limb.set_joint_position_speed(0.2)
self.rate = rospy.Rate(10)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.jnt_st_sub = rospy.Subscriber('/robot/joint_states',
JointState,
self._update_jnt_state,
queue_size=1)
self.jnt_cm_pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=None)
self.jnt_ee_sub = rospy.Subscriber('/robot/limb/right/endpoint_state',
EndpointState,
self.update_ee_pose,
queue_size=1)
'''
Three points on the object in "right_gripper_base" frame.
Obect is static in gripper frame. Needs to be calculated only once
during init time
'''
self.obj_pose1 = PoseStamped()
self.obj_pose1.header.frame_id = "right_gripper_base"
self.obj_pose1.pose.position.x = self.obj_x1
self.obj_pose1.pose.position.y = self.obj_y1
self.obj_pose1.pose.position.z = self.obj_z1
self.obj_pose2 = PoseStamped()
self.obj_pose2.header.frame_id = "right_gripper_base"
self.obj_pose2.pose.position.x = self.obj_x2
self.obj_pose2.pose.position.y = self.obj_y2
self.obj_pose2.pose.position.z = self.obj_z2
self.obj_pose3 = PoseStamped()
self.obj_pose3.header.frame_id = "right_gripper_base"
self.obj_pose3.pose.position.x = self.obj_x3
self.obj_pose3.pose.position.y = self.obj_y3
self.obj_pose3.pose.position.z = self.obj_z3
'''
All members of the observation vector have to be provided
'''
def _set_cur_obs(self, obs):
#self._print_env_log('Waiting for obs lock')
self.obs_lock.acquire()
try:
#self._print_env_log('Acquired obs lock')
self.cur_obs = copy.copy(obs)
finally:
#self._print_env_log('Released obs lock')
self.obs_lock.release()
def _get_cur_obs(self):
self.obs_lock.acquire()
try:
#self._print_env_log('Acquired obs lock')
obs = copy.copy(self.cur_obs)
finally:
#self._print_env_log('Released obs lock')
self.obs_lock.release()
return obs
def close_env_log(self):
self.logp.close()
self.logp = None
def _print_env_log(self, string):
if self.train_mode:
if self.logp is None:
return
self.logp.write("\n")
now = rospy.get_rostime()
t_str = time.strftime("%H-%M")
t_str += "-" + str(now.secs) + "-" + str(now.nsecs) + ": "
self.logp.write(t_str + string)
self.logp.write("\n")
def _load_inits(self, path):
if self.train_mode:
# Store versions of the main code required for
# test and debug after training
copyfile(path + "/init.yaml", self.logpath + "/init.yaml")
copyfile(path + "/ros_env.py", self.logpath + "/ros_env.py")
stream = open(path + "/init.yaml", "r")
config = yaml.load(stream)
stream.close()
self.distance_thresh = config['distance_thresh']
# limits for the uniform distribution to
# sample from when varying initial joint states during training
# joint position limits have to be in ascending order of joint number
# jnt_init_limits is a ordered list of [lower_limit, upper_limit] for
# each joint in motion
# limits for the joint positions
self.jnt_init_limits = config['jnt_init_limits']
self.cmd_mode = config['cmd_mode']
if self.cmd_mode == 'VELOCITY_MODE':
# limits for the joint positions
self.jnt_pos_limits = config['jnt_pos_limits']
self.vel_limit = config['vel_limit']
self.vel_mode = config['vel_mode']
else:
self.torque_limit = config['torque_limit']
'''
# The following are the names of Sawyer's joints that will move
# for the shape sorting cube task
# Any one or more of the following in ascending order -
# ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
# 'right_j5', 'right_j6']
# The last joint is the gripper finger joint and stays fixed
'''
self.debug_lvl = config['debug_lvl']
self.cmd_names = config['cmd_names']
self.init_pos = config['initial-position']
self.goal_obs_dim = config['goal_obs_dim']
'''
In TORQUE_MODE, jnt_obs_dim will be twice the size of the
number of joints being controlled (2 * self.act_dim), one each for
position and velocity.
The ordering in the observation vector is:
j0:pos, j1:pos, ..., jN:pos,
j0:vel, j1:vel ...., jN:vel, # Applicable only in torque mode
obj_coord:x1, obj_coord:y1, obj_coord:z1,
obj_coord:x2, obj_coord:y2, obj_coord:z2,
obj_coord:x3, obj_coord:y3, obj_coord:z3;
goal_coord:x1, goal_coord:y1, goal_coord:z1,
goal_coord:x2, goal_coord:y2, goal_coord:z2,
goal_coord:x3, goal_coord:y3, goal_coord:z3
'''
self.jnt_obs_dim = config['jnt_obs_dim']
self.obs_dim = self.goal_obs_dim + self.jnt_obs_dim
self.act_dim = config['act_dim']
'''
These indices have to be in ascending order
The length of this ascending order list >=1 and <=7 (values 0 to 6)
The last joint is the gripper finger joint and stays fixed
'''
self.jnt_indices = config['jnt_indices']
# test time goals specified in jnt angle space (for now)
if not self.train_mode:
self.test_goal = config['test_goal']
self.max_training_goals = config['max_training_goals']
self.batch_size = config['min_timesteps_per_batch']
self.goal_XYZs = config['goal_XYZs']
'''
The following 9 object coordinates are in "right_gripper_base" frame
They are relayed by the camera observer at init time.
They can be alternatively read from the pg_init.yaml file too
'''
self.obj_x1 = config['obj_x1']
self.obj_y1 = config['obj_y1']
self.obj_z1 = config['obj_z1']
self.obj_x2 = config['obj_x2']
self.obj_y2 = config['obj_y2']
self.obj_z2 = config['obj_z2']
self.obj_x3 = config['obj_x3']
self.obj_y3 = config['obj_y3']
self.obj_z3 = config['obj_z3']
# Callback invoked when EE pose update message is received
# This function will update the pose of object in gripper
def update_ee_pose(self, msg):
try:
tform = self.tf_buffer.lookup_transform("base",
"right_gripper_base",
rospy.Time(),
rospy.Duration(1.0))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
self._print_env_log("TF Exception, not storing update")
return
obs = self._get_cur_obs()
trans = (tform.transform.translation.x, tform.transform.translation.y,
tform.transform.translation.z)
rot = (tform.transform.rotation.x, tform.transform.rotation.y,
tform.transform.rotation.z, tform.transform.rotation.w)
mat44 = np.dot(transformations.translation_matrix(trans),
transformations.quaternion_matrix(rot))
pose44 = np.dot(xyz_to_mat44(self.obj_pose1.pose.position),
xyzw_to_mat44(self.obj_pose1.pose.orientation))
txpose = np.dot(mat44, pose44)
xyz = tuple(transformations.translation_from_matrix(txpose))[:3]
x, y, z = xyz
obs[self.jnt_obs_dim] = x
obs[self.jnt_obs_dim + 1] = y
obs[self.jnt_obs_dim + 2] = z
pose44 = np.dot(xyz_to_mat44(self.obj_pose2.pose.position),
xyzw_to_mat44(self.obj_pose2.pose.orientation))
txpose = np.dot(mat44, pose44)
xyz = tuple(transformations.translation_from_matrix(txpose))[:3]
x, y, z = xyz
obs[self.jnt_obs_dim + 3] = x
obs[self.jnt_obs_dim + 4] = y
obs[self.jnt_obs_dim + 5] = z
pose44 = np.dot(xyz_to_mat44(self.obj_pose3.pose.position),
xyzw_to_mat44(self.obj_pose3.pose.orientation))
txpose = np.dot(mat44, pose44)
xyz = tuple(transformations.translation_from_matrix(txpose))[:3]
x, y, z = xyz
obs[self.jnt_obs_dim + 6] = x
obs[self.jnt_obs_dim + 7] = y
obs[self.jnt_obs_dim + 8] = z
'''
self._print_env_log("EE coordinates: "
+ str(msg.pose.position.x) +
", " + str(msg.pose.position.y) + ", " +
str(msg.pose.position.z))
self._print_env_log("Obj location: "
+ str(obs[self.jnt_obs_dim:self.jnt_obs_dim+9]))
'''
self._set_cur_obs(obs)
def _update_jnt_state(self, msg):
'''
Only care for mesgs which have length 9
There is a length 1 message for the gripper finger joint
which we dont care about
'''
if len(msg.position) != 9:
return
obs = self._get_cur_obs()
enum_iter = enumerate(self.jnt_indices, start=0)
for i, index in enum_iter:
'''
Need to add a 1 to message index as it starts from head_pan
[head_pan, j0, j1, .. torso]
whereas joint_indices are starting from j0
'''
obs[i] = msg.position[index + 1]
if self.cmd_mode == "TORQUE_MODE":
obs[i + self.jnt_obs_dim / 2] = msg.velocity[index + 1]
self._set_cur_obs(obs)
'''
This function is called from reset only and during both training and testing
'''
def _init_jnt_state(self):
q_jnt_angles = copy.copy(self.init_pos)
# Command Sawyer's joints to pre-set angles and velocity
# JointCommand.msg mode: TRAJECTORY_MODE
positions = dict()
# Build some randomness in starting position
# between each subsequent iteration
enum_iter = enumerate(self.jnt_indices, start=0)
for i, index in enum_iter:
l_limit = self.jnt_init_limits[i][0]
u_limit = self.jnt_init_limits[i][1]
val = np.random.uniform(l_limit, u_limit)
q_jnt_angles[index] = val
string = "Initializing joint states to: "
enum_iter = enumerate(self.all_jnts, start=0)
for i, jnt_name in enum_iter:
positions[jnt_name] = q_jnt_angles[i]
string += str(positions[jnt_name]) + " "
self.limb.move_to_joint_positions(positions, 30)
self._print_env_log(string)
# sleep for a bit to ensure that the joints reach commanded positions
rospy.sleep(3)
def _action_clip(self, action):
if self.cmd_mode == "TORQUE_MODE":
return action
#return np.clip(action, -self.torque_limit, self.torque_limit)
else:
return np.clip(action, -self.vel_limit, self.vel_limit)
def _set_joint_velocities(self, actions):
if self.vel_mode == "raw":
velocities = dict()
enum_iter = enumerate(self.cmd_names, start=0)
for i, jnt in enum_iter:
velocities[jnt] = actions[i]
command_msg = JointCommand()
command_msg.names = velocities.keys()
command_msg.velocity = velocities.values()
command_msg.mode = JointCommand.VELOCITY_MODE
command_msg.header.stamp = rospy.Time.now()
self.jnt_cm_pub.publish(command_msg)
else:
# Command Sawyer's joints to angles as calculated by velocity*dt
positions = dict()
q_jnt_angles = copy.copy(self.init_pos)
obs_prev = self._get_cur_obs()
enum_iter = enumerate(self.jnt_indices, start=0)
for i, index in enum_iter:
timestep = self.dt + np.random.normal(0, 1)
val = obs_prev[i] + actions[i] * timestep
val = np.clip(val, self.jnt_pos_limits[i][0],
self.jnt_pos_limits[i][1])
q_jnt_angles[index] = val
enum_iter = enumerate(self.all_jnts, start=0)
for i, jnt_name in enum_iter:
positions[jnt_name] = q_jnt_angles[i]
self.limb.move_to_joint_positions(positions)
def _set_joint_torques(self, actions):
torques = dict()
enum_iter = enumerate(self.all_jnts, start=0)
for i, jnt_name in enum_iter:
torques[jnt_name] = 0.0
enum_iter = enumerate(self.cmd_names, start=0)
for i, jnt_name in enum_iter:
torques[jnt_name] = actions[i]
command_msg = JointCommand()
command_msg.names = torques.keys()
command_msg.effort = torques.values()
command_msg.mode = JointCommand.TORQUE_MODE
command_msg.header.stamp = rospy.Time.now()
self.jnt_cm_pub.publish(command_msg)
def step(self, action):
self.cur_act = copy.deepcopy(action)
# called to take a step with the provided action
# send the action as generated by policy (clip before sending)
clipped_acts = self._action_clip(action)
if self.cmd_mode == "TORQUE_MODE":
self._set_joint_torques(clipped_acts)
else:
self._set_joint_velocities(clipped_acts)
'''
NOTE: Observations are being constantly updated because
we are subscribed to the robot state publisher and also
subscribed to the ee topic which calculates
the pose of the goal and the block.
Sleep for some time, so that the action
execution on the robot finishes and we wake up to
pick up the latest observation
'''
# no sleep necessary if we send velocity integrated positions
if self.cmd_mode == "TORQUE_MODE" or self.vel_mode == "raw":
self.rate.sleep()
obs = self._get_cur_obs()
diff = self._get_diff(obs)
done = self._is_done(diff)
self.cur_reward = self._calc_reward(diff, done)
return obs, self.cur_reward, done
def _set_cartesian_test_goal(self):
self.goal_pos_x1 = self.test_goal[0][0]
self.goal_pos_y1 = self.test_goal[0][1]
self.goal_pos_z1 = self.test_goal[0][2]
self.goal_pos_x1 = self.test_goal[1][0]
self.goal_pos_y1 = self.test_goal[1][1]
self.goal_pos_z1 = self.test_goal[1][2]
self.goal_pos_x1 = self.test_goal[2][0]
self.goal_pos_y1 = self.test_goal[2][1]
self.goal_pos_z1 = self.test_goal[2][2]
def _set_random_training_goal(self):
k = self.goal_cntr
l = -0.01
u = 0.01
self.goal_pos_x1 = self.goal_XYZs[k][0][0] + np.random.uniform(l, u)
self.goal_pos_y1 = self.goal_XYZs[k][0][1] + np.random.uniform(l, u)
self.goal_pos_z1 = self.goal_XYZs[k][0][2] + np.random.uniform(l, u)
self.goal_pos_x2 = self.goal_XYZs[k][1][0] + np.random.uniform(l, u)
self.goal_pos_y2 = self.goal_XYZs[k][1][1] + np.random.uniform(l, u)
self.goal_pos_z2 = self.goal_XYZs[k][1][2] + np.random.uniform(l, u)
self.goal_pos_x3 = self.goal_XYZs[k][2][0] + np.random.uniform(l, u)
self.goal_pos_y3 = self.goal_XYZs[k][2][1] + np.random.uniform(l, u)
self.goal_pos_z3 = self.goal_XYZs[k][2][2] + np.random.uniform(l, u)
def reset(self):
# called Initially when the Env is initialized
# set the initial joint state and send the command
if not self.train_mode:
self._set_cartesian_test_goal()
else:
if self.goal_cntr == self.max_training_goals - 1:
self.goal_cntr = 0
else:
self.goal_cntr += 1
self._set_random_training_goal()
cur_obs = self._get_cur_obs()
# Update cur_obs with the new goal
od = self.jnt_obs_dim
cur_obs[od + 9] = self.goal_pos_x1
cur_obs[od + 10] = self.goal_pos_y1
cur_obs[od + 11] = self.goal_pos_z1
cur_obs[od + 12] = self.goal_pos_x2
cur_obs[od + 13] = self.goal_pos_y2
cur_obs[od + 14] = self.goal_pos_z2
cur_obs[od + 15] = self.goal_pos_x3
cur_obs[od + 16] = self.goal_pos_y3
cur_obs[od + 17] = self.goal_pos_z3
self._set_cur_obs(cur_obs)
# this call will result in sleeping for 3 seconds
self._init_jnt_state()
# send the latest observations
return self._get_cur_obs()
def _get_diff(self, obs):
od = self.jnt_obs_dim
diff = [
abs(obs[od + 0] - obs[od + 9]),
abs(obs[od + 1] - obs[od + 10]),
abs(obs[od + 2] - obs[od + 11]),
abs(obs[od + 3] - obs[od + 12]),
abs(obs[od + 4] - obs[od + 13]),
abs(obs[od + 5] - obs[od + 14]),
abs(obs[od + 6] - obs[od + 15]),
abs(obs[od + 7] - obs[od + 16]),
abs(obs[od + 8] - obs[od + 17])
]
return diff
def _is_done(self, diff):
# all elements of d are positive values
done = all(d <= self.distance_thresh for d in diff)
if done:
self._print_env_log(" Reached the goal!!!! ")
return done
def _calc_reward(self, diff, done):
l2 = np.linalg.norm(np.array(diff))
l2sq = l2**2
alpha = 1e-6
w_l2 = -1e-3
w_u = -1e-2
w_log = -1.0
reward = 0.0
dist_cost = l2sq
precision_cost = np.log(l2sq + alpha)
cntrl_cost = np.square(self.cur_act).sum()
reward += w_l2 * dist_cost + w_log * precision_cost + w_u * cntrl_cost
string = "l2sq: {}, log of l2sq: {} contrl_cost: {} reward: {}".format(
dist_cost, precision_cost, cntrl_cost, reward)
self._print_env_log(" Current Reward: " + string)
return reward
def main():
"""
Move the robot arm to the specified configuration.
Call using:
$ rosrun intera_examples go_to_cartesian_pose.py [arguments: see below]
-p 0.4 -0.3 0.18 -o 0.0 1.0 0.0 0.0 -t right_hand
--> Go to position: x=0.4, y=-0.3, z=0.18 meters
--> with quaternion orientation (0, 1, 0, 0) and tip name right_hand
--> The current position or orientation will be used if only one is provided.
-q 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0
--> Go to joint angles: 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 using default settings
--> If a Cartesian pose is not provided, Forward kinematics will be used
--> If a Cartesian pose is provided, the joint angles will be used to bias the nullspace
-R 0.01 0.02 0.03 0.1 0.2 0.3 -T
-> Jog arm with Relative Pose (in tip frame)
-> x=0.01, y=0.02, z=0.03 meters, roll=0.1, pitch=0.2, yaw=0.3 radians
-> The fixed position and orientation paramters will be ignored if provided
"""
epilog = """
See help inside the example with the '?' key for key bindings.
"""
rp = intera_interface.RobotParams()
valid_limbs = rp.get_limb_names()
if not valid_limbs:
rp.log_message(("Cannot detect any limb parameters on this robot. "
"Exiting."), "ERROR")
return
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__,
epilog=epilog)
parser.add_argument("-r",
"--record_point_pose",
type=bool,
default=0,
nargs='+',
help="record pose or not")
parser.add_argument("-p",
"--position",
type=float,
nargs='+',
help="Desired end position: X, Y, Z")
parser.add_argument("-o",
"--orientation",
type=float,
nargs='+',
help="Orientation as a quaternion (x, y, z, w)")
parser.add_argument(
"-R",
"--relative_pose",
type=float,
nargs='+',
help=
"Jog pose by a relative amount in the base frame: X, Y, Z, roll, pitch, yaw"
)
parser.add_argument(
"-T",
"--in_tip_frame",
action='store_true',
help="For relative jogs, job in tip frame (default is base frame)")
parser.add_argument(
"-q",
"--joint_angles",
type=float,
nargs='+',
default=[],
help="A list of joint angles, one for each of the 7 joints, J0...J6")
parser.add_argument("-t",
"--tip_name",
default='right_hand',
help="The tip name used by the Cartesian pose")
parser.add_argument("--linear_speed",
type=float,
default=0.6,
help="The max linear speed of the endpoint (m/s)")
parser.add_argument(
"--linear_accel",
type=float,
default=0.6,
help="The max linear acceleration of the endpoint (m/s/s)")
parser.add_argument(
"--rotational_speed",
type=float,
default=1.57,
help="The max rotational speed of the endpoint (rad/s)")
parser.add_argument(
"--rotational_accel",
type=float,
default=1.57,
help="The max rotational acceleration of the endpoint (rad/s/s)")
parser.add_argument(
"--timeout",
type=float,
default=None,
help=
"Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout."
)
parser.add_argument(
"-l",
"--limb",
dest="limb",
default=valid_limbs[0],
choices=valid_limbs,
help="Limb on which to run the gripper keyboard example")
args = parser.parse_args(rospy.myargv()[1:])
try:
rospy.init_node('go_to_cartesian_pose_py')
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions(
max_linear_speed=args.linear_speed,
max_linear_accel=args.linear_accel,
max_rotational_speed=args.rotational_speed,
max_rotational_accel=args.rotational_accel,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_names = limb.joint_names()
rate = rospy.Rate(10) # 10hz
if args.record_point_pose:
origin_trans = get_position_now(limb)
return None
base_quaternions = np.array([0.0, 0.0, 1.0, 0.0])
bin_point = np.array([0.35216134766, 0.621893054464, 0.371810527511])
base_angle = 0.0
while not rospy.is_shutdown():
ch = raw_input("waiting for next step:")
if ch == 'r':
origin_trans = get_position_now(limb)
if ch == 'g':
map_keyboard(args.limb)
if ch == 'p':
coordinates_text = raw_input("type in coordinates:")
base_angle = float(coordinates_text)
# if float(coordinates_text)>np.pi:
# base_rotated_angle=2*np.pi-float(coordinates_text)
# else:
# base_rotated_angle=np.pi-float(coordinates_text)
# base_firest=np.array([0.0,0.0,1.0,0.0])
# base_quaternions=quaternions_after_rotation(base_firest,base_rotated_angle)
# print("After reset the rotated, base quaternions are:")
# print(base_quaternions)
if ch == 'drop':
coordinates_text = raw_input("type in coordinates:")
bin_point = np.array([
float(coordinates_text.split()[0]),
float(coordinates_text.split()[1]),
float(coordinates_text.split()[2])
])
if ch == 'q':
return None
if ch == 'd':
only_grip(args.limb, 'q')
coordinates_text = raw_input("type in coordinates:")
target_point = np.array([
float(coordinates_text.split()[0]),
float(coordinates_text.split()[1]),
float(coordinates_text.split()[2])
])
pick_angle = float(coordinates_text.split()[3])
pick_angle = float(coordinates_text.split()[3]) - base_angle
pick_angle = -pick_angle
temp = base_quaternions
target_after = quaternions_after_rotation(temp, pick_angle)
grip_rotated_this_time = target_after
pick_quaternions = [
float(grip_rotated_this_time[1]),
float(grip_rotated_this_time[2]),
float(grip_rotated_this_time[3]),
float(grip_rotated_this_time[0])
]
traj_1 = MotionTrajectory(trajectory_options=traj_options,
limb=limb)
waypoint_1 = MotionWaypoint(options=wpt_opts.to_msg(),
limb=limb)
result_up = go_to_the_point(
target_point,
args=args,
limb=limb,
waypoint=waypoint_1,
traj=traj_1,
target_quaterniond=pick_quaternions)
only_grip(args.limb, 'o')
matrix = quaternions.quat2mat([
pick_quaternions[3], pick_quaternions[0],
pick_quaternions[1], pick_quaternions[2]
])
down_point = target_point + np.dot(matrix, down_distance)
traj_2 = MotionTrajectory(trajectory_options=traj_options,
limb=limb)
waypoint_2 = MotionWaypoint(options=wpt_opts.to_msg(),
limb=limb)
result_down = go_to_the_point(
down_point,
args=args,
limb=limb,
waypoint=waypoint_2,
traj=traj_2,
target_quaterniond=pick_quaternions)
only_grip(args.limb, 'q')
traj_3 = MotionTrajectory(trajectory_options=traj_options,
limb=limb)
waypoint_3 = MotionWaypoint(options=wpt_opts.to_msg(),
limb=limb)
result_up = go_to_the_point(
target_point,
args=args,
limb=limb,
waypoint=waypoint_3,
traj=traj_3,
target_quaterniond=pick_quaternions)
traj_4 = MotionTrajectory(trajectory_options=traj_options,
limb=limb)
waypoint_4 = MotionWaypoint(options=wpt_opts.to_msg(),
limb=limb)
result_up = go_to_the_point(
bin_point,
args,
limb,
waypoint_4,
traj_4,
target_quaterniond=pick_quaternions)
only_grip(args.limb, 'o')
if ch == 's':
coordinates_text = raw_input(
"type in coordinates and rotation angle:")
target_point = [
float(coordinates_text.split()[0]),
float(coordinates_text.split()[1]),
float(coordinates_text.split()[2])
]
rotatain_angle_from_base = float(
coordinates_text.split()[3]) - base_angle
rotatain_angle_from_base = -rotatain_angle_from_base
temp = base_quaternions
target_after = quaternions_after_rotation(
temp, rotatain_angle_from_base)
grip_rotated_this_time = target_after
target_quaterniond = [
float(grip_rotated_this_time[1]),
float(grip_rotated_this_time[2]),
float(grip_rotated_this_time[3]),
float(grip_rotated_this_time[0])
]
traj_new = MotionTrajectory(trajectory_options=traj_options,
limb=limb)
waypoint_new = MotionWaypoint(options=wpt_opts.to_msg(),
limb=limb)
result = go_to_the_point(target_point,
args=args,
limb=limb,
waypoint=waypoint_new,
traj=traj_new,
target_quaterniond=target_quaterniond)
if result is None:
rospy.logerr('Trajectory FAILED to send')
return
if result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!'
)
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
def moveTo(myArgs):
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main_server.__doc__)
parser.add_argument(
"-p", "--position", type=float,
nargs='+',
help="Desired end position: X, Y, Z")
parser.add_argument(
"-o", "--orientation", type=float,
nargs='+',
help="Orientation as a quaternion (x, y, z, w)")
parser.add_argument(
"-R", "--relative_pose", type=float,
nargs='+',
help="Jog pose by a relative amount in the base frame: X, Y, Z, roll, pitch, yaw")
parser.add_argument(
"-T", "--in_tip_frame", action='store_true',
help="For relative jogs, job in tip frame (default is base frame)")
parser.add_argument(
"-q", "--joint_angles", type=float,
nargs='+', default=[],
help="A list of joint angles, one for each of the 7 joints, J0...J6")
parser.add_argument(
"--timeout", type=float, default=None,
help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.")
args = parser.parse_args(myArgs.call.split(" "))
print(args.position)
#test_string = ['-p','0.5', '0.3', '0.5']
#args = parser.parse_args(test_string)
try:
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options = traj_options, limb = limb)
wpt_opts = MotionWaypointOptions(max_linear_speed=0.4,
max_linear_accel=0.4,
max_rotational_speed=1.57,
max_rotational_accel=1.5,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
joint_names = limb.joint_names()
if args.joint_angles and len(args.joint_angles) != len(joint_names):
rospy.logerr('len(joint_angles) does not match len(joint_names!)')
return "failed"
if (args.position is None and args.orientation is None
and args.relative_pose is None):
if args.joint_angles:
# does Forward Kinematics
waypoint.set_joint_angles(args.joint_angles,'right_hand', joint_names)
else:
rospy.loginfo("No Cartesian pose or joint angles given. Using default")
waypoint.set_joint_angles(joint_angles=None, active_endpoint='right_hand')
else:
endpoint_state = limb.tip_state('right_hand')
if endpoint_state is None:
rospy.logerr('Endpoint state not found with tip name %s', 'right_hand')
return "failed"
pose = endpoint_state.pose
if args.relative_pose is not None:
if len(args.relative_pose) != 6:
rospy.logerr('Relative pose needs to have 6 elements (x,y,z,roll,pitch,yaw)')
return "failed"
# create kdl frame from relative pose
rot = PyKDL.Rotation.RPY(args.relative_pose[3],
args.relative_pose[4],
args.relative_pose[5])
trans = PyKDL.Vector(args.relative_pose[0],
args.relative_pose[1],
args.relative_pose[2])
f2 = PyKDL.Frame(rot, trans)
# and convert the result back to a pose message
if args.in_tip_frame:
# end effector frame
pose = posemath.toMsg(posemath.fromMsg(pose) * f2)
else:
# base frame
pose = posemath.toMsg(f2 * posemath.fromMsg(pose))
else:
if args.position is not None and len(args.position) == 3:
pose.position.x = args.position[0]
pose.position.y = args.position[1]
pose.position.z = args.position[2]
if args.orientation is not None and len(args.orientation) == 4:
pose.orientation.x = args.orientation[0]
pose.orientation.y = args.orientation[1]
pose.orientation.z = args.orientation[2]
pose.orientation.w = args.orientation[3]
poseStamped = PoseStamped()
poseStamped.pose = pose
waypoint.set_cartesian_pose(poseStamped, 'right_hand', args.joint_angles)
rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string())
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=args.timeout)
if result is None:
rospy.logerr('Trajectory FAILED to send')
return 'Trajectory FAILED to send'
if result.result:
rospy.loginfo('Motion controller successfully finished the trajectory!')
return 'Motion Success'
else:
rospy.logerr('Motion controller failed to complete the trajectory with error %s',
result.errorId)
return result.errorId
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.')
return "failed"
def main():
"""
Send a random-walk trajectory, starting from the current pose of the robot.
Can be used to send both joint and cartesian trajectories.
WARNING: Make sure the surrounding area around the robot is collision free
prior to sending random trajectories.
Call using:
$ rosrun intera_examples send_random_trajectory.py [arguments: see below]
-n 5 -t JOINT -s 0.5
--> Send a trandom joint trajectory with 5 waypoints, using a speed ratio
of 0.5 for all waypoints. Use default random walk settings.
-d 0.1 -b 0.2
--> Send a random trajectory with default trajectory settings. Use a
maximum step distance of 0.1*(upper joint limit - lower joint limit)
and avoid the upper and lower joint limits by a boundary of
0.2*(upper joint limit - lower joint limit).
-o ~/Desktop/fileName.bag
--> Save the trajectory message to a bag file
-p
--> Prints the trajectory to terminal before sending
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument(
"-n", "--waypoint_count", type=int, default=5,
help="number of waypoints to include in the trajectory")
parser.add_argument(
"-d", "--stepDistance", type=float, default=0.05,
help="normalized random walk step distance")
parser.add_argument(
"-b", "--boundaryPadding", type=float, default=0.1,
help="normalized padding to apply to joint limits")
parser.add_argument(
"-t", "--trajType", type=str, default='JOINT',
choices=['JOINT', 'CARTESIAN'],
help="trajectory interpolation type")
parser.add_argument(
"-s", "--speed_ratio", type=float, default=0.5,
help="A value between 0.0 (slow) and 1.0 (fast)")
parser.add_argument(
"-a", "--accel_ratio", type=float, default=0.5,
help="A value between 0.0 (slow) and 1.0 (fast)")
parser.add_argument(
"-o", "--output_file",
help="Save the trajectory task to a bag file")
parser.add_argument(
"--output_file_type", default="yaml",
choices=["csv", "yaml"], help="Select the save file type")
parser.add_argument(
"-p", "--print_trajectory", action='store_true', default=False,
help="print the trajectory after loading")
parser.add_argument(
"--do_not_send", action='store_true', default=False,
help="generate the trajectory, but do not send to motion controller.")
parser.add_argument(
"--log_file_output",
help="Save motion controller log messages to this file name")
parser.add_argument(
"--timeout", type=float, default=None,
help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.")
args = parser.parse_args()
if args.waypoint_count < 1:
args.waypoint_count = 1
rospy.logwarn('Input out of bounds! Setting waypoint_count = 1')
try:
rospy.init_node('send_random_joint_trajectory_py')
if not args.do_not_send:
rospy.logwarn('Make sure the surrounding area around the robot is '
'collision free prior to sending random trajectories.')
k = input("Press 'Enter' when the robot is clear to continue...")
if k:
rospy.logerr("Please press only the 'Enter' key to begin execution. Exiting...")
sys.exit(1)
# Set the trajectory options
limb = Limb()
traj_opts = TrajectoryOptions()
traj_opts.interpolation_type = args.trajType
traj = MotionTrajectory(trajectory_options = traj_opts, limb = limb)
# Set the waypoint options
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=args.speed_ratio,
max_joint_accel=args.accel_ratio)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
# Append a waypoint at the current pose
waypoint.set_joint_angles(limb.joint_ordered_angles())
traj.append_waypoint(waypoint.to_msg())
# Set up the random walk generator
walk = RandomWalk()
limits = JointLimits()
walk.set_lower_limits(limits.get_joint_lower_limits(limb.joint_names()))
walk.set_upper_limits(limits.get_joint_upper_limits(limb.joint_names()))
walk.set_last_point(waypoint.get_joint_angles())
walk.set_boundary_padding(args.boundaryPadding)
walk.set_maximum_distance(args.stepDistance)
for i in range(0, args.waypoint_count):
joint_angles = walk.get_next_point()
waypoint.set_joint_angles(joint_angles = joint_angles)
traj.append_waypoint(waypoint.to_msg())
if args.output_file is not None:
if args.output_file_type == "csv":
traj.to_csv_file(args.output_file)
elif args.output_file_type == "yaml":
traj.to_yaml_file(args.output_file)
else:
rospy.logwarn("Did not recognize output file type")
if args.print_trajectory:
rospy.loginfo('\n' + traj.to_string())
if args.log_file_output is not None:
traj.set_log_file_name(args.log_file_output)
if not args.do_not_send:
result = traj.send_trajectory(timeout=args.timeout)
if result is None:
rospy.logerr('Trajectory FAILED to send')
elif result.result:
rospy.loginfo('Motion controller successfully finished the trajectory!')
else:
rospy.logerr('Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user. '
'Exiting before trajectory completion.')