def _guarded_move_to_joint_position_two(self, set_joint_angles, speed_ratio = 0.4, accel_ratio = 0.2, timeout = 10.0):
try:
traj = MotionTrajectory(limb = self._limb)
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=speed_ratio,
max_joint_accel=accel_ratio)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = self._limb)
joint_angles = self._limb.joint_ordered_angles()
waypoint.set_joint_angles(joint_angles = joint_angles)
traj.append_waypoint(waypoint.to_msg())
if len(set_joint_angles) != len(joint_angles):
rospy.logerr('The number of joint_angles must be %d', len(joint_angles))
return None
waypoint.set_joint_angles(joint_angles = set_joint_angles)
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 move_to_joint_positions(self,
positions,
speed=None,
acceleration=None):
speed = speed if speed is not None else self.speed
acceleration = acceleration if acceleration is not None else self.acceleration
traj = MotionTrajectory(limb=self.limb)
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=speed,
max_joint_accel=acceleration)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=self.limb)
if isinstance(positions, dict):
joint_angles = positions.values()
waypoint.set_joint_angles(joint_angles=joint_angles)
traj.set_joint_names(positions.keys())
traj.append_waypoint(waypoint.to_msg())
else:
rospy.logerr("Incorrect inputs to move_to_joint_positions")
return
result = traj.send_trajectory(timeout=30)
self.last_activity = rospy.Time.now()
if result is None:
rospy.logerr("Trajectory failed to send")
elif not result.result:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
def linear_movement(self,position, linear_speed = 0.2, linear_accel = 0.2, rotational_speed = 0.1, rotational_accel = 0.1):
try:
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options = traj_options, limb = self._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=0.2)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = self._limb)
poseStamped = PoseStamped()
poseStamped.pose = position
waypoint.set_cartesian_pose(poseStamped, self._tip_name)
rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string())
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=5.0)
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 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)
def joint_angles(args):
# arg_fmt = argparse.RawDescriptionHelpFormatter
# parser = argparse.ArgumentParser(formatter_class=arg_fmt,
# description=main.__doc__)
# parser.add_argument(
# "-q", "--joint_angles", type=float,
# nargs='+', default=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0],
# help="A list of joint angles, one for each of the 7 joints, J0...J6")
# parser.add_argument(
# "-s", "--speed_ratio", type=float, default=0.5,
# help="A value between 0.001 (slow) and 1.0 (maximum joint velocity)")
# parser.add_argument(
# "-a", "--accel_ratio", type=float, default=0.5,
# help="A value between 0.001 (slow) and 1.0 (maximum joint accel)")
# 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_joint_angles_py')
limb = Limb()
traj = MotionTrajectory(limb=limb)
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)
joint_angles = limb.joint_ordered_angles()
waypoint.set_joint_angles(joint_angles=joint_angles)
traj.append_waypoint(waypoint.to_msg())
if len(args.joint_angles) != len(joint_angles):
rospy.logerr('The number of joint_angles must be %d',
len(joint_angles))
return None
waypoint.set_joint_angles(joint_angles=args.joint_angles)
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!')
return True
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 move(self, point_list, wait = True, MAX_LIN_SPD=7.0, MAX_LIN_ACCL=1.5): # one point = [x_coord, y_coord, z_coord, x_deg, y_deg, z_deg]
try:
limb = Limb() # point_list = [pointA, pointB, pointC, ...]
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
except:
print("There may have been an error while exiting")
if self.STOP:
traj.stop_trajectory()
return True
wpt_opts = MotionWaypointOptions(max_linear_speed=MAX_LIN_SPD, max_linear_accel=MAX_LIN_ACCL, corner_distance=0.002)
for point in point_list:
q_base = quaternion_from_euler(0, 0, math.pi/2)
#q_rot = quaternion_from_euler(math.radians(point[3]), math.radians(point[4]), math.radians(point[5]))
q_rot = quaternion_from_euler(point[3], point[4], point[5])
q = quaternion_multiply(q_rot, q_base)
newPose = PoseStamped()
newPose.header = Header(stamp=rospy.Time.now(), frame_id='base')
newPose.pose.position.x = point[0] + 0.65
newPose.pose.position.y = point[1] + 0.0
newPose.pose.position.z = point[2] + 0.4
newPose.pose.orientation.x = q[0]
newPose.pose.orientation.y = q[1]
newPose.pose.orientation.z = q[2]
newPose.pose.orientation.w = q[3]
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
waypoint.set_cartesian_pose(newPose, "right_hand", limb.joint_ordered_angles())
traj.append_waypoint(waypoint.to_msg())
if(wait):
print(" \n --- Sending trajectory and waiting for finish --- \n")
result = traj.send_trajectory(wait_for_result=wait)
if result is None:
rospy.logerr('Trajectory FAILED to send')
success = False
elif result.result:
rospy.loginfo('Motion controller successfully finished the trajcetory')
success = True
else:
rospy.logerr('Motion controller failed to complete the trajectory. Error: %s', result.errorId)
success = False
else:
print("\n --- Sending trajector w/out waiting --- \n")
traj.send_trajectory(wait_for_result=wait)
success = True
return success
def go_to_pose(self, position, orientation):
try:
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options,
limb=self._right_arm)
wpt_opts = MotionWaypointOptions(max_linear_speed=0.6,
max_linear_accel=0.6,
max_rotational_speed=1.57,
max_rotational_accel=1.57,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(),
limb=self._right_arm)
pose = Pose()
pose.position.x = position[0]
pose.position.y = position[1]
pose.position.z = position[2]
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[0]
poseStamped = PoseStamped()
poseStamped.pose = pose
joint_angles = self._right_arm.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(timeout=10)
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 execute_trajectory(self,
trajectory,
joint_names,
speed=None,
acceleration=None):
"""
trajectory is a list of points: approach (joint), init (cart), drawing (cart), retreat (cart)
"""
speed = speed if speed is not None else self.speed
acceleration = acceleration if acceleration is not None else self.acceleration
if isinstance(trajectory, dict) and "approach" in trajectory:
for mtype in ["approach", "init", "drawing", "retreat"]:
points = trajectory[mtype]
if points["type"] == "joint":
self._seed = points["joints"]
self.move_to_joint_positions(
dict(zip(joint_names, points["joints"])))
elif points["type"] == "cart":
wpt_opts = MotionWaypointOptions(
max_joint_speed_ratio=speed,
max_joint_accel=acceleration)
traj = MotionTrajectory(limb=self.limb)
waypoint = MotionWaypoint(options=wpt_opts, limb=self.limb)
t_opt = TrajectoryOptions(
interpolation_type=TrajectoryOptions.CARTESIAN)
jv = deepcopy(points["joints"])
jv.reverse()
waypoint.set_joint_angles(jv)
waypoint.set_cartesian_pose(
list_to_pose_stamped(points["pose"], frame_id="base"))
traj.append_waypoint(waypoint)
jn = [str(j) for j in joint_names]
jn.reverse()
traj.set_joint_names(jn)
traj.set_trajectory_options(t_opt)
result = traj.send_trajectory(timeout=10)
self.last_activity = rospy.Time.now()
if result is None:
rospy.logerr("Trajectory failed to send")
elif not result.result:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
else:
rospy.logwarn("Unknown type %", mtype)
else:
rospy.logerr("Incorrect inputs to execute_trajectory")
return
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 go_to_joint_angles(
self,
joint_angles=[math.pi / 2, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0],
speed_ratio=0.5,
accel_ratio=0.5,
timeout=None,
ways=False):
try:
if isinstance(joint_angles, dict):
joint_angles_arr = [0] * len(joint_angles)
for j in range(len(joint_angles_arr)):
joint_angles_arr[j] = joint_angles['right_j' + str(j)]
joint_angles = joint_angles_arr
traj = MotionTrajectory(limb=self)
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=speed_ratio,
max_joint_accel=accel_ratio)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=self)
joint_angles_start = self.joint_ordered_angles()
waypoint.set_joint_angles(joint_angles=joint_angles_start)
traj.append_waypoint(waypoint.to_msg())
if len(joint_angles) != len(joint_angles_start):
rospy.logerr('The number of joint_angles must be %d',
len(joint_angles_start))
return None
waypoint.set_joint_angles(joint_angles=joint_angles)
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!')
return True
else:
#rospy.logerr('Motion controller failed to complete the trajectory with error %s',result.errorId)
if ways == True:
rospy.loginfo('Collision anticipated')
self.go_to_joint_angles()
return True
else:
rospy.loginfo(
'Could not complete trajectory due to collision')
return False
except rospy.ROSInterruptException:
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
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 move_move(limb, group, target, speed_ratio=None, accel_ratio=None, timeout=None):
if speed_ratio is None:
speed_ratio = 0.3
if accel_ratio is None:
accel_ratio = 0.1
plan = group.plan(target)
# rospy.sleep(1)
step = []
for point in plan.joint_trajectory.points:
step.append(point.positions)
traj = MotionTrajectory(limb=limb)
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=speed_ratio,
max_joint_accel=accel_ratio)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
for point in step:
waypoint.set_joint_angles(joint_angles=point)
traj.append_waypoint(waypoint.to_msg())
traj.send_trajectory(timeout=timeout)
group.stop()
group.clear_pose_targets()
def go_to_angles(joint_angles_goal, speed_ratio_goal, accel_ratio_goal,
timeout_goal):
try:
#rospy.init_node('go_to_joint_angles_py') # initialze once
limb = Limb()
traj = MotionTrajectory(limb=limb)
wpt_opts = MotionWaypointOptions(
max_joint_speed_ratio=speed_ratio_goal,
max_joint_accel=accel_ratio_goal)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_angles = limb.joint_ordered_angles()
waypoint.set_joint_angles(joint_angles=joint_angles)
traj.append_waypoint(waypoint.to_msg())
if len(joint_angles_goal) != len(joint_angles):
rospy.logerr('The number of joint_angles must be %d',
len(joint_angles))
return None
waypoint.set_joint_angles(joint_angles=joint_angles_goal)
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=timeout_goal)
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.'
)
class Robot(object):
def __init__(self, joint_names):
self._motion = MotionTrajectory()
self.joint_names = joint_names
def execute(self, trajectory):
self._motion.clear_waypoints()
self._motion.set_joint_names(self.joint_names)
for point in trajectory:
waypoint = MotionWaypoint()
waypoint.set_joint_angles(point)
self._motion.append_waypoint(waypoint)
print(self._motion.to_msg())
return self._motion.send_trajectory()
def move_to_joint_positions(self, positions):
self._motion.clear_waypoints()
self._motion.set_joint_names(positions.keys())
waypoint = MotionWaypoint()
waypoint.set_joint_angles(positions.values())
self._motion.append_waypoint(waypoint)
return self._motion.send_trajectory()
def move_move_haha(limb, group, positions, speed_ratio=None, accel_ratio=None, timeout=None):
# this move method is obtained from Intera motion control interface
# limb -- the limb object of the Sawyer robot
# position -- a list with a length of 7, specifying the goal state joint position
# speed_ratio -- the speed of robot arm, [0, 1]
# accel_ratio -- the acceleration of the robot arm, [0, 1]
# timeout -- the timeout for this specific motion; infinite when None
rospy.sleep(1)
if accel_ratio is None:
accel_ratio = 0.1
if speed_ratio is None:
speed_ratio = 0.3
traj = MotionTrajectory(limb=limb)
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=speed_ratio,
max_joint_accel=accel_ratio)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
waypoint.set_joint_angles(joint_angles=positions)
traj.append_waypoint(waypoint.to_msg())
traj.send_trajectory(timeout=timeout)
rospy.sleep(1)
pose_goal.orientation.y = OR_Y
pose_goal.orientation.z = OR_Z
pose_goal.position.x = BASE_X
pose_goal.position.y = BASE_Y
pose_goal.position.z = BASE_Z
poseStamped = PoseStamped()
poseStamped.pose = pose_goal
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
joint_angles = limb.joint_ordered_angles()
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
waypoint.set_cartesian_pose(poseStamped, 'right_hand', joint_angles)
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=5.0)
rate = rospy.Rate(30.0) # SUGGEST - Try 10 or more
while not rospy.is_shutdown():
try:
(trans, rot) = listener.lookupTransform('kinect/user_1/torso',
'kinect/user_1/right_hand',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.loginfo('user_1 not found:' + str(type(e)))
try:
(trans,
rot) = listener.lookupTransform('kinect/user_2/torso',
def interaction_joint_trajectory(
limb, joint_angles, trajType, interaction_active,
interaction_control_mode, interaction_frame, speed_ratio, accel_ratio,
K_impedance, max_impedance, in_endpoint_frame, force_command,
K_nullspace, endpoint_name, timeout, disable_damping_in_force_control,
disable_reference_resetting, rotations_for_constrained_zeroG):
try:
traj = MotionTrajectory(limb=limb)
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=speed_ratio,
max_joint_accel=accel_ratio)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
# one single waypoint
current_joint_angles = limb.joint_ordered_angles()
waypoint.set_joint_angles(joint_angles=current_joint_angles)
traj.append_waypoint(waypoint.to_msg())
if len(current_joint_angles) != len(joint_angles):
rospy.logerr('The number of joint_angles must be %d',
len(current_joint_angles))
return None
# ----- testing intermediate points with real robot
middle_joint_angles = [
(current_joint_angles[i] + joint_angles[i]) / 2.0
for i in xrange(len(current_joint_angles))
]
waypoint.set_joint_angles(joint_angles=middle_joint_angles)
traj.append_waypoint(waypoint.to_msg())
waypoint.set_joint_angles(joint_angles=joint_angles)
traj.append_waypoint(waypoint.to_msg())
# ----- end testing intermediate points with real robot
# set the interaction control options in the current configuration
interaction_options = InteractionOptions()
trajectory_options = TrajectoryOptions()
trajectory_options.interaction_control = True
trajectory_options.interpolation_type = trajType
interaction_options.set_interaction_control_active(
int2bool(interaction_active))
interaction_options.set_K_impedance(K_impedance)
interaction_options.set_max_impedance(int2bool(max_impedance))
interaction_options.set_interaction_control_mode(
interaction_control_mode)
interaction_options.set_in_endpoint_frame(int2bool(in_endpoint_frame))
interaction_options.set_force_command(force_command)
interaction_options.set_K_nullspace(K_nullspace)
interaction_options.set_endpoint_name(endpoint_name)
if len(interaction_frame) < 7:
rospy.logerr('The number of elements must be 7!')
elif len(interaction_frame) == 7:
quat_sum_square = interaction_frame[3] * interaction_frame[
3] + interaction_frame[4] * interaction_frame[
4] + interaction_frame[5] * interaction_frame[
5] + interaction_frame[6] * interaction_frame[6]
if quat_sum_square < 1.0 + 1e-7 and quat_sum_square > 1.0 - 1e-7:
target_interaction_frame = Pose()
target_interaction_frame.position.x = interaction_frame[0]
target_interaction_frame.position.y = interaction_frame[1]
target_interaction_frame.position.z = interaction_frame[2]
target_interaction_frame.orientation.w = interaction_frame[3]
target_interaction_frame.orientation.x = interaction_frame[4]
target_interaction_frame.orientation.y = interaction_frame[5]
target_interaction_frame.orientation.z = interaction_frame[6]
interaction_options.set_interaction_frame(
target_interaction_frame)
else:
rospy.logerr(
'Invalid input to quaternion! The quaternion must be a unit quaternion!'
)
else:
rospy.logerr('Invalid input to interaction_frame!')
interaction_options.set_disable_damping_in_force_control(
disable_damping_in_force_control)
interaction_options.set_disable_reference_resetting(
disable_reference_resetting)
interaction_options.set_rotations_for_constrained_zeroG(
rotations_for_constrained_zeroG)
trajectory_options.interaction_params = interaction_options.to_msg()
traj.set_trajectory_options(trajectory_options)
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 with interaction options set!'
)
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
# print the resultant interaction options
rospy.loginfo('Interaction Options:\n%s', interaction_options.to_msg())
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user. %s',
'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 joint_angles_in_contact(input_arg):
"""
Move the robot arm to the specified configuration
with the desired interaction control options.
Call using:
$ rosrun intera_examples go_to_joint_angles_in_contact.py [arguments: see below]
-q 0.0 0.0 0.0 0.0 0.0 0.0 0.0
--> Go to joint pose: 0.0 0.0 0.0 0.0 0.0 0.0 0.0 using default settings
-q 0.1 -0.2 0.15 -0.05 -0.08 0.14 -0.04 -s 0.1
--> Go to pose [...] with a speed ratio of 0.1
-q -0.2 0.1 0.1 0.2 -0.3 0.2 0.4 -s 0.9 -a 0.1
--> Go to pose [...] witha speed ratio of 0.9 and an accel ratio of 0.1
--trajType CARTESIAN
--> Use a Cartesian interpolated endpoint path to reach the goal
=== Interaction Mode options ===
-st 1
--> Set the interaction controller state (1 for True, 0 for False) in the current configuration
-k 500.0 500.0 500.0 10.0 10.0 10.0
--> Set K_impedance to [500.0 500.0 500.0 10.0 10.0 10.0] in the current configuration
-m 0
--> Set max_impedance to False for all 6 directions in the current configuration
-m 1 1 0 1 1 1
--> Set max_impedance to [True True False True True True] in the current configuration
-kn 5.0 3.0 5.0 4.0 6.0 4.0 6.0
--> Set K_nullspace to [5.0 3.0 5.0 4.0 6.0 4.0 6.0] in the current configuration
-f 0.0 0.0 30.0 0.0 0.0 0.0
--> Set force_command to [0.0 0.0 30.0 0.0 0.0 0.0] in the current configuration
-ef
--> Set in_endpoint_frame to True in the current configuration
-en 'right_hand'
--> Specify the desired endpoint frame where impedance and force control behaviors are defined
-md 1
--> Set interaction_control_mode to impedance mode for all 6 directions in the current configuration
(1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit)
-md 1 1 2 1 1 1
--> Set interaction_control_mode to [impedance, impedance, force, impedance, impedance, impedance] in the current configuration
(1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit)
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(
formatter_class=arg_fmt, description="joint_angles_in_contact.__doc__")
parser.add_argument(
"-q",
"--joint_angles",
type=float,
nargs='+',
default=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0],
help="A list of joint angles, one for each of the 7 joints, J0...J6")
parser.add_argument(
"-s",
"--speed_ratio",
type=float,
default=0.1,
help="A value between 0.001 (slow) and 1.0 (maximum joint velocity)")
parser.add_argument(
"-a",
"--accel_ratio",
type=float,
default=0.5,
help="A value between 0.001 (slow) and 1.0 (maximum joint accel)")
parser.add_argument("-t",
"--trajType",
type=str,
default='JOINT',
choices=['JOINT', 'CARTESIAN'],
help="trajectory interpolation type")
parser.add_argument(
"-st",
"--interaction_active",
type=int,
default=1,
choices=[0, 1],
help="Activate (1) or Deactivate (0) interaction controller")
parser.add_argument(
"-k",
"--K_impedance",
type=float,
nargs='+',
default=[1300.0, 1300.0, 1300.0, 30.0, 30.0, 30.0],
help=
"A list of desired stiffnesses, one for each of the 6 directions -- stiffness units are (N/m) for first 3 and (Nm/rad) for second 3 values"
)
parser.add_argument(
"-m",
"--max_impedance",
type=int,
nargs='+',
default=[1, 1, 1, 1, 1, 1],
choices=[0, 1],
help=
"A list of impedance modulation state, one for each of the 6 directions (a single value can be provided to apply the same value to all the directions) -- 0 for False, 1 for True"
)
parser.add_argument(
"-md",
"--interaction_control_mode",
type=int,
nargs='+',
default=[1, 1, 1, 1, 1, 1],
choices=[1, 2, 3, 4],
help=
"A list of desired interaction control mode (1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit), one for each of the 6 directions"
)
parser.add_argument(
"-fr",
"--interaction_frame",
type=float,
nargs='+',
default=[0, 0, 0, 1, 0, 0, 0],
help=
"Specify the reference frame for the interaction controller -- first 3 values are positions [m] and last 4 values are orientation in quaternion (w, x, y, z)"
)
parser.add_argument(
"-ef",
"--in_endpoint_frame",
action='store_true',
default=False,
help=
"Set the desired reference frame to endpoint frame; otherwise, it is base frame by default"
)
parser.add_argument(
"-en",
"--endpoint_name",
type=str,
default='right_hand',
help=
"Set the desired endpoint frame by its name; otherwise, it is right_hand frame by default"
)
parser.add_argument(
"-f",
"--force_command",
type=float,
nargs='+',
default=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
help=
"A list of desired force commands, one for each of the 6 directions -- in force control mode this is the vector of desired forces/torques to be regulated in (N) and (Nm), in impedance with force limit mode this vector specifies the magnitude of forces/torques (N and Nm) that the command will not exceed"
)
parser.add_argument(
"-kn",
"--K_nullspace",
type=float,
nargs='+',
default=[5.0, 10.0, 5.0, 10.0, 5.0, 10.0, 5.0],
help=
"A list of desired nullspace stiffnesses, one for each of the 7 joints (a single value can be provided to apply the same value to all the directions) -- units are in (Nm/rad)"
)
parser.add_argument("-dd",
"--disable_damping_in_force_control",
action='store_true',
default=False,
help="Disable damping in force control")
parser.add_argument(
"-dr",
"--disable_reference_resetting",
action='store_true',
default=False,
help=
"The reference signal is reset to actual position to avoid jerks/jumps when interaction parameters are changed. This option allows the user to disable this feature."
)
parser.add_argument(
"-rc",
"--rotations_for_constrained_zeroG",
action='store_true',
default=False,
help=
"Allow arbitrary rotational displacements from the current orientation for constrained zero-G (use only for a stationary reference orientation)"
)
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(input_arg[1:])
try:
#rospy.init_node('go_to_joint_angles_in_contact_py')
limb = Limb()
traj = MotionTrajectory(limb=limb)
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)
joint_angles = limb.joint_ordered_angles()
waypoint.set_joint_angles(joint_angles=joint_angles)
traj.append_waypoint(waypoint.to_msg())
if len(args.joint_angles) != len(joint_angles):
rospy.logerr('The number of joint_angles must be %d',
len(joint_angles))
return None
waypoint.set_joint_angles(joint_angles=args.joint_angles)
traj.append_waypoint(waypoint.to_msg())
# set the interaction control options in the current configuration
interaction_options = InteractionOptions()
trajectory_options = TrajectoryOptions()
trajectory_options.interaction_control = True
trajectory_options.interpolation_type = args.trajType
interaction_options.set_interaction_control_active(
int2bool(args.interaction_active))
interaction_options.set_K_impedance(args.K_impedance)
interaction_options.set_max_impedance(int2bool(args.max_impedance))
interaction_options.set_interaction_control_mode(
args.interaction_control_mode)
interaction_options.set_in_endpoint_frame(
int2bool(args.in_endpoint_frame))
interaction_options.set_force_command(args.force_command)
interaction_options.set_K_nullspace(args.K_nullspace)
interaction_options.set_endpoint_name(args.endpoint_name)
if len(args.interaction_frame) < 7:
rospy.logerr('The number of elements must be 7!')
elif len(args.interaction_frame) == 7:
quat_sum_square = args.interaction_frame[
3] * args.interaction_frame[3] + args.interaction_frame[
4] * args.interaction_frame[4]
+args.interaction_frame[5] * args.interaction_frame[
5] + args.interaction_frame[6] * args.interaction_frame[6]
if quat_sum_square < 1.0 + 1e-7 and quat_sum_square > 1.0 - 1e-7:
interaction_frame = Pose()
interaction_frame.position.x = args.interaction_frame[0]
interaction_frame.position.y = args.interaction_frame[1]
interaction_frame.position.z = args.interaction_frame[2]
interaction_frame.orientation.w = args.interaction_frame[3]
interaction_frame.orientation.x = args.interaction_frame[4]
interaction_frame.orientation.y = args.interaction_frame[5]
interaction_frame.orientation.z = args.interaction_frame[6]
interaction_options.set_interaction_frame(interaction_frame)
else:
rospy.logerr(
'Invalid input to quaternion! The quaternion must be a unit quaternion!'
)
else:
rospy.logerr('Invalid input to interaction_frame!')
interaction_options.set_disable_damping_in_force_control(
args.disable_damping_in_force_control)
interaction_options.set_disable_reference_resetting(
args.disable_reference_resetting)
interaction_options.set_rotations_for_constrained_zeroG(
args.rotations_for_constrained_zeroG)
trajectory_options.interaction_params = interaction_options.to_msg()
traj.set_trajectory_options(trajectory_options)
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 with interaction options set!'
)
return True
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
# print the resultant interaction options
rospy.loginfo('Interaction Options:\n%s', interaction_options.to_msg())
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user. %s',
'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.'
)
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.')
def main():
"""
Move the robot arm to the specified configuration.
Call using:
$ rosrun intera_examples go_to_joint_angles.py [arguments: see below]
-q 0.0 0.0 0.0 0.0 0.0 0.0 0.0
--> Go to joint pose: 0.0 0.0 0.0 0.0 0.0 0.0 0.0 using default settings
-q 0.1 -0.2 0.15 -0.05 -0.08 0.14 -0.04 -s 0.1
--> Go to pose [...] with a speed ratio of 0.1
-q -0.2 0.1 0.1 0.2 -0.3 0.2 0.4 -s 0.9 -a 0.1
--> Go to pose [...] with a speed ratio of 0.9 and an accel ratio of 0.1
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument(
"-q",
"--joint_angles",
type=float,
nargs='+',
default=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0],
help="A list of joint angles, one for each of the 7 joints, J0...J6")
parser.add_argument(
"-s",
"--speed_ratio",
type=float,
default=0.5,
help="A value between 0.001 (slow) and 1.0 (maximum joint velocity)")
parser.add_argument(
"-a",
"--accel_ratio",
type=float,
default=0.5,
help="A value between 0.001 (slow) and 1.0 (maximum joint accel)")
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(rospy.myargv()[1:])
try:
rospy.init_node('go_to_joint_angles_py')
limb = Limb()
traj = MotionTrajectory(limb=limb)
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)
joint_angles = limb.joint_ordered_angles()
waypoint.set_joint_angles(joint_angles=joint_angles)
traj.append_waypoint(waypoint.to_msg())
if len(args.joint_angles) != len(joint_angles):
rospy.logerr('The number of joint_angles must be %d',
len(joint_angles))
return None
waypoint.set_joint_angles(joint_angles=args.joint_angles)
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 main():
# if len(sys.argv) < 4:
# print('{0} <BindIP><Server IP><Message>'.format(sys.argv[0]))
# sys.exit()
bindIP = '192.168.101.5' #sys.argv[1]
serverIP = '192.168.101.12' #sys.argv[2]
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # SOCK_STREAM은 TCP socket을 뜻함
sock.bind((bindIP, 0))
sock.connect((serverIP, 5425)) # 서버에 연결 요청
rospy.init_node('sawyer_client')
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
# parser.add_argument(
# "-q", "--joint_angles", type=float,
# nargs='+', default=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0],
# help="A list of joint angles, one for each of the 7 joints, J0...J6")
parser.add_argument(
"-s", "--speed_ratio", type=float, default=0.5,
help="A value between 0.001 (slow) and 1.0 (maximum joint velocity)")
parser.add_argument(
"-a", "--accel_ratio", type=float, default=0.5,
help="A value between 0.001 (slow) and 1.0 (maximum joint accel)")
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(rospy.myargv()[1:])
goal_joint_angles = [0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while True:
# 서버로 부터 수신
rbuff = sock.recv(1024) # 메시지 수신
received = str(rbuff)
# print('수신:{0}'.format(received))
if received.count >= 5:
center_str = received.split('SM')[1].split('SE')[0]
print(center_str)
if center_str == 'Q':
break
elif center_str == 'A':
goal_joint_angles = [0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0]
elif center_str == 'B':
goal_joint_angles = [0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
limb = Limb()
traj = MotionTrajectory(limb = limb)
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)
joint_angles = limb.joint_ordered_angles()
waypoint.set_joint_angles(joint_angles = joint_angles)
traj.append_waypoint(waypoint.to_msg())
waypoint.set_joint_angles(joint_angles = goal_joint_angles)
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=args.timeout)
sock.close()
class MySawyer(object):
#
# Init class
def __init__(self, name='MySawyer', limb='right', anonymous=True, disable_signals=True, light=True, gripper_reverse=False):
rospy.init_node(name, anonymous=anonymous, disable_signals=disable_signals)
# rospy.sleep(1)
#
#
self._limb=None
self._head=None
self._light=None
self._head_display=None
self._display=None
self._cuff=None
self._limits=None
self._navigator=None
self._init_nodes(limb,light)
self._get_gripper(gripper_reverse)
#
# Default Variables
self._home_pos=[0.0, -1.178, 0.0, 2.178, 0.0, 0.567, 3.313]
self._init_pos=[0.0, -1.178, 0.0, 2.178, 0.0, 0.567, 3.313]
self._default_pos=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0]
self._motion_trajectory=None
#self._joint_names=self._limb.joint_names()
#self._velocity_limits=self._limits.joint_velocity_limits()
#
# for motion controller
self._motions={}
self._joint_positions={'home':self._home_pos,'init':self._init_pos, 'default':self._default_pos}
self._index=0
self._p_index=0
self._is_recording=False
self.max_record_time=30
self._accuracy=0.01 # 0.05 this value use velocity control mode and position control mode
self._recording_intval=0.5
#
# for velicity control mode
self._running=True
self._target=[0.0, -1.178, 0.0, 2.178, 0.0, 0.567, 3.313] ### initial position
self._target_motion=[]
self._vmax=0.4
self._vrate=2.0
self._is_moving=False
#
# for interaction mode
self._speed_ratio=0.1 # 0.001 -- 1.0
self._max_speed_ratio=0.5 # 0.001 -- 1.0
self._max_accel_ratio=0.5 # 0.001 -- 1.0
self._trajType='JOINT' # 'JOINT' ot 'CARTESIAN'
self._interaction_active=True
self._K_impedance=[1300.0,1300.0, 1300.0, 30.0, 30.0, 30.0]
self._max_impedance=[1,1,1,1,1,1]
self._interaction_control_mode=[1,1,1,1,1,1]
self._interaction_frame=[0,0,0,1,0,0,0]
self._in_endpoint_frame=False
self._endpoint_name='right_hand'
self._force_command=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self._K_nullspace=[5.0, 10.0, 5.0, 10.0, 5.0, 10.0, 5.0]
self._disable_damping_in_force_control=False
self._disable_reference_resetting=False
self._rotations_for_constrained_zeroG=False
self._timeout=None
# for Cartesian Pose base motion
self._in_tip_frame=False
self._tip_name='right_hand'
self._linear_speed=0.6 # m/s
self._linear_accel=0.6 # m/s/s
self._rotational_speed=1.57 # rad/s
self._rotational_accel=1.57 # rad/s/s
## for event handlers
self.ok_id=None
self.show_id=None
self.back_id=None
#
# for RTC
self._is_pause=False
#
#
def _init_nodes(self, limb, light):
try:
self._limb=intera_interface.Limb(limb)
self._head=intera_interface.Head()
self._light=SawyerLight(light)
#self._head_display=intera_interface.HeadDisplay()
self._display=SawyerDisplay()
self._cuff=intera_interface.Cuff()
self._limits=intera_interface.JointLimits()
self._navigator=intera_interface.Navigator()
self._joint_names=self._limb.joint_names()
self._velocity_limits=self._limits.joint_velocity_limits()
self._stop_cmd={}
for i,name in enumerate(self._joint_names):
self._stop_cmd[name]=0.0
except:
print("Warning caught exception...")
traceback.print_exc()
pass
#
3
def _get_gripper(self, gripper_reverse):
try:
self._gripper=intera_interface.get_current_gripper_interface()
self._is_clicksmart = isinstance(self._gripper, intera_interface.SimpleClickSmartGripper)
self._gripper_reverse=gripper_reverse
if self._is_clicksmart:
if self._gripper.needs_init():
self._gripper.initialize()
_signals=self._gripper.get_ee_signals()
if 'grip' in _signals:
self._gripper_type='grip'
elif 'vacuumOn' in _signals:
self._gripper_type='vacuum'
else:
self._gripper_type='unknown'
else:
if not (self._gripper.is_calibrated() or
self._gripper.calibrate() == True):
raise
except:
self._gripper=None
self._is_clicksmart=False
self._gripper_type=None
self._gripper_reverse=None
#
#
def activate(self):
#
# Enable Robot
self._rs=intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
self._init_state=self._rs.state().enabled
self._rs.enable()
#
# current positions
self._angles=self._limb.joint_angles()
self._pose=self._limb.endpoint_pose()
#
#
self._limb.set_joint_position_speed(self._speed_ratio)
#
# LED white ON
self._light.head_on()
self.mkRosPorts()
self.set_record()
#
#
def mkRosPorts(self):
self._sub=dict()
self._pub=dict()
self._sub['target_joint_pos']=rospy.Subscriber('target_joint_pos', String,self.set_target_joint_pos)
self._pub['current_joint_pos']=rospy.Publisher('current_joint_pos', String,queue_size=1)
self._pub['target_joint_pos']=rospy.Publisher('target_joint_pos', String,queue_size=1)
self._pub['image']=rospy.Publisher('/robot/head_display', Image, latch=True, queue_size=10)
#
#
def set_motion_sequencer(self):
self.set_subscriber('current_joint_pos', self.set_next_target, String)
#
#
def set_subscriber(self, name, func, arg_type=String):
if name in self._sub and self._sub[name]: self._sub[name].unregister()
self._sub[name]=rospy.Subscriber(name, arg_type,func)
def unset_subscriber(self, name):
if name in self._sub and self._sub[name]: self._sub[name].unregister()
self._sub[name]=None
#
#
def get_joint_angles(self):
return self._limb.joint_ordered_angles()
def get_joint_velocities(self):
vel=self._limb.joint_velocities()
return map(lambda x: vel[x], self._joint_names)
#
#
def enable(self):
self._rs.enable()
#
#
def state(self):
print(self._rs.state())
#
#
def reset(self):
self._rs.reset()
#
#
def disable(self):
self._rs.disable()
#
#
def stop(self):
self._rs.stop()
#
#
def exit_control_mode(self):
self._limb.exit_control_mode()
#
#
def update_pose(self):
self._angles=self._limb.joint_angles()
self._pose=self._limb.endpoint_pose()
#
#
def init_pos(self, use_motion_ctrl=True):
if use_motion_ctrl:
self.move_to([self._init_pos])
else:
self._light.head_green()
self._limb.move_to_neutral(speed=self._speed_ratio)
self.update_pose()
self._light.head_on()
#
#
def set_speed(self, rate=0.3):
self._speed_ratio=rate
self._limb.set_joint_position_speed(rate)
#
#
def print_joiint_pos(self, dtime=5.0, intval=0.1):
end_time = rospy.Time.now() + rospy.Duration(dtime)
while rospy.Time.now() < end_time:
if rospy.is_shutdown() : break
print(self._limb.endpoint_pose())
rospy.sleep(intval)
##############################################
# Joint Position Control (Depreciated for Intera 5.2 and beyond)
def move_joints(self, pos):
self._limb.set_joint_position_speed(self._speed_ratio)
self._light.head_green()
self._limb.move_to_joint_positions(pos)
self.update_pose()
self._light.head_on()
#
#
def move_cart(self, x_dist, y_dist, z_dist):
self._limb.set_joint_position_speed(self._speed_ratio)
self._pose=self.endpoint_pose()
self._pose.position.x += x_dist
self._pose.position.y += y_dist
self._pose.position.z += z_dist
self.move_joints(self._limb.ik_request(self._pose))
#
#
def record_motion(self, name=None, dtime=0, intval=1.0):
if not name :
name=self.mk_motion_name()
self._index += 1
if dtime <= 0:
dtime=self.max_record_time
print ("Start Recording:", name)
self._light.head_blue()
self._motions[name]=[]
self._is_recording=True
end_time = rospy.Time.now() + rospy.Duration(dtime)
while (rospy.Time.now() < end_time) and self._is_recording :
if rospy.is_shutdown() : break
self._motions[name].append(self._limb.joint_angles())
rospy.sleep(intval)
print ("End Recording: record ", len(self._motions[name]), " points")
self._is_recording=False
self._light.head_on()
#
#
def mk_motion_name(self):
name = 'Motion_' + str(self._index)
while name in self._motions:
self._index += 1
name = 'Motion_' + str(self._index)
return name
#
# Record positions
def record_pos(self,val):
if val :
self._light.head_yellow()
name = 'P_' + str(self._p_index)
while name in self._joint_positions:
self._p_index += 1
name = 'P_' + str(self._p_index)
self._joint_positions[name]=self._limb.joint_ordered_angles()
self._light.head_on()
#
# Motion Recorder Event Handleer(Start)
def start_record(self, value):
if value:
print('Start..')
self.record_motion(None, 0, self._recording_intval)
#
# Motion Recorder Event Handler(Stop)
def stop_record(self, value):
if value:
print('Stop..')
self._is_recording=False
#
# set Event Handlers
def set_record(self):
print ("Register callbacks")
self.ok_id=self._navigator.register_callback(self.start_record, 'right_button_ok')
self.back_id=self._navigator.register_callback(self.stop_record, 'right_button_back')
self.square_id=self._navigator.register_callback(self.record_pos, 'right_button_square')
self.show_id=self._navigator.register_callback(self.unset_record, 'right_button_show')
#
# unset Event Handlers
def unset_record(self, value=0):
if value and self.ok_id :
print ("Unregister all callbacks")
if self._navigator.deregister_callback(self.ok_id) : self.ok_id=None
#if self._navigator.deregister_callback(self.show_id) : self.show_id=None
if self._navigator.deregister_callback(self.back_id) : self.back_id=None
if self._navigator.deregister_callback(self.square_id) : self.square_id=None
def gripper_state(self):
if self.is_gripping :
return 1
else:
return 0
#######################################################
#
# For Joint Position mode (before SDK-5.2)
def play_motion(self, name, intval=0.0):
self._limb.set_joint_position_speed(self._speed_ratio)
self._light.head_green()
for pos in self._motions[name]:
if rospy.is_shutdown() :
self._light.head_red()
return
#
self._limb.move_to_joint_positions(pos, threshold=self._accuracy)
if intval > 0: rospy.sleep(intval)
self._light.head_on()
#
#
def play_motion_seq(self, names):
self._limb.set_joint_position_speed(self._speed_ratio)
self._light.head_green()
for name in names:
for pos in self._motions[name]:
if rospy.is_shutdown() :
self._light.head_red()
return
self._limb.move_to_joint_positions(pos)
self._light.head_on()
###############################################
#
#
def list_motions(self):
print(self._motions.keys())
#
#
def joint_pos_d2l(self, pos):
return map(lambda x: pos[x], self._joint_names)
def convert_motion(self, name):
return map(lambda x: self.joint_pos_d2l(x), self._motions[name])
#
#
def save_motion(self, name):
with open("motions/"+name+".jpos", mode="w") as f:
for pos in self._motions[name]:
f.write(str(pos))
f.write("\n")
#
#
def load_motion(self, name):
self._motions[name]=[]
with open("motions/"+name+".jpos") as f:
motion=f.readlines()
for p in motion:
self._motions[name].append( eval(p) )
#
#
def get_joint_positions(self, name):
if type(name) == str:
if name in self._joint_positions:
target_joints=self._joint_positions[name]
else:
print("Invalid position name")
target_joints=None
elif len(name) == 7:
target_joints=name
return target_joints
####################################
#
# Move Motion
def move_to(self, name=None, tout=None, with_in_contact=False, wait_for_result=True):
#
# for Motion Controller Interface
if type(name) == str and name in self._motions:
waypoints=self.convert_motion(name)
elif type(name) == list:
waypoints=name
else:
print("Invalid motion name")
return None
self._motion_trajectory=MotionTrajectory(limb=self._limb)
_wpt_opts=MotionWaypointOptions(max_joint_speed_ratio=self._max_speed_ratio,
max_joint_accel=self._max_accel_ratio)
_waypoint=MotionWaypoint(options=_wpt_opts, limb=self._limb)
#
# set current joint position...
_waypoint.set_joint_angles(joint_angles=self._limb.joint_ordered_angles())
self._motion_trajectory.append_waypoint(_waypoint.to_msg())
#
# set target joint position...
for pos in waypoints:
if type(pos) == str:
if pos in self._joint_positions:
pos=self._joint_positions[pos]
_waypoint.set_joint_angles(joint_angles=pos)
self._motion_trajectory.append_waypoint(_waypoint.to_msg())
else:
_waypoint.set_joint_angles(joint_angles=pos)
self._motion_trajectory.append_waypoint(_waypoint.to_msg())
#
#
if with_in_contact :
opts=self.get_in_contact_opts()
if opts :
self._motion_trajectory.set_trajectory_options(opts)
#
# run motion...
self._light.head_green()
result=self._motion_trajectory.send_trajectory(wait_for_result=wait_for_result,timeout=tout)
#
#
if result is None:
self._light.head_yellow()
print("Trajectory FAILED to send")
return None
#
#
if not wait_for_result : return True
#
if result.result: self._light.head_on()
else: self._light.head_red()
#
#
self._motion_trajectory=None
return result.result
#
# Move in Certecian Mode
def cart_move_to(self, target_pos, tout=None, relative_mode=False, wait_for_result=True):
#
# for Motion Controller Interface
_trajectory_opts=TrajectoryOptions()
_trajectory_opts.interpolation_type=TrajectoryOptions.CARTESIAN
#
self._motion_trajectory=MotionTrajectory(trajectory_options=_trajectory_opts, limb=self._limb)
#
# set Waypoint Options
_wpt_opts=MotionWaypointOptions(max_linear_speed=self._linear_speed,
max_linear_accel=self._linear_accel,
max_rotational_speed=self._rotational_speed,
max_rotational_accel=self._rotational_accel,
max_joint_speed_ratio=1.0)
_waypoint=MotionWaypoint(options=_wpt_opts, limb=self._limb)
#
endpoint_state=self._limb.tip_state(self._tip_name)
pose=endpoint_state.pose
########################################
# set target position
if relative_mode:
# relative position : target_pos -> x, y, z, roll, pitch, yew
trans = PyKDL.Vector(target_pos[0],target_pos[1],target_pos[2])
rot = PyKDL.Rotation.RPY(target_pos[3], target_pos[4],target_pos[5])
f2 = PyKDL.Frame(rot, trans)
if self._in_tip_frame:
# endpoint's cartesian systems
pose=posemath.toMsg(posemath.fromMsg(pose) * f2)
else:
# base's cartesian systems
pose=posemath.toMsg(f2 * posemath.fromMsg(pose))
else:
# global position : x, y, z, rx, ry, rz, rw
pose.position.x=target_pos[0]
pose.position.y=target_pos[1]
pose.position.z=target_pos[2]
pose.orientation.x=target_pos[3]
pose.orientation.y=target_pos[4]
pose.orientation.z=target_pos[5]
pose.orientation.w=target_pos[6]
#
###########################################
# set target position.
poseStamped=PoseStamped()
poseStamped.pose=pose
_waypoint.set_cartesian_pose(poseStamped, self._tip_name, [])
self._motion_trajectory.append_waypoint(_waypoint.to_msg())
#
# run motion...
self._light.head_green()
result=self._motion_trajectory.send_trajectory( wait_for_result=wait_for_result,timeout=tout)
#
if result is None:
self._light.head_yellow()
print("Trajectory FAILED to send")
return None
#
if not wait_for_result : return True
#
if result.result: self._light.head_on()
else: self._light.head_red()
#
self._motion_trajectory=None
return result.result
#
# stop motion...
def stop_trajectory(self):
if self._motion_trajectory :
self._motion_trajectory.stop_trajectory()
#
# set Interaction control
def set_interaction_params(self):
interaction_options = InteractionOptions()
interaction_options.set_interaction_control_active(self._interaction_active)
interaction_options.set_K_impedance(self._K_impedance)
interaction_options.set_max_impedance(self._max_impedance)
interaction_options.set_interaction_control_mode(self._interaction_control_mode)
interaction_options.set_in_endpoint_frame(self._in_endpoint_frame)
interaction_options.set_force_command(self._force_command)
interaction_options.set_K_nullspace(self._K_nullspace)
interaction_options.set_endpoint_name(self._endpoint_name)
if len(self._interaction_frame) == 7:
quat_sum_square = self._interaction_frame[3]*self._interaction_frame[3] + self._interaction_frame[4]*self._interaction_frame[4] + self._interaction_frame[5]*self._interaction_frame[5] + self._interaction_frame[6]*self._interaction_frame[6]
if quat_sum_square < 1.0 + 1e-7 and quat_sum_square > 1.0 - 1e-7:
interaction_frame = Pose()
interaction_frame.position.x = self._interaction_frame[0]
interaction_frame.position.y = self._interaction_frame[1]
interaction_frame.position.z = self._interaction_frame[2]
interaction_frame.orientation.w = self._interaction_frame[3]
interaction_frame.orientation.x = self._interaction_frame[4]
interaction_frame.orientation.y = self._interaction_frame[5]
interaction_frame.orientation.z = self._interaction_frame[6]
interaction_options.set_interaction_frame(interaction_frame)
else:
print('Invalid input to quaternion! The quaternion must be a unit quaternion!')
return None
else:
print('Invalid input to interaction_frame!')
return None
interaction_options.set_disable_damping_in_force_control(self._disable_damping_in_force_control)
interaction_options.set_disable_reference_resetting(self._disable_reference_resetting)
interaction_options.set_rotations_for_constrained_zeroG(self._rotations_for_constrained_zeroG)
return interaction_options
#
#
def set_interaction_mode(self):
pub = rospy.Publisher('/robot/limb/right/interaction_control_command', InteractionControlCommand, queue_size = 1)
interaction_options = self.set_interaction_params()
if interaction_options:
msg=interaction_options.to_msg()
pub.publish(msg)
#
#
def get_in_contact_opts(self):
interaction_options = self.set_interaction_params()
if interaction_options:
trajectory_options = TrajectoryOptions()
trajectory_options.interaction_control = True
trajectory_options.interpolation_type = self._trajType
trajectory_options.interaction_params = interaction_options.to_msg()
return trajectory_options
else:
return None
##############################################################
#
# Open the gripper
def gripper_open(self):
if self._gripper and self._gripper.is_ready():
if self._is_clicksmart:
if 'grip' in self._gripper.get_ee_signals() :
self._gripper.set_ee_signal_value('grip', self._gripper_reverse)
else:
return None
else:
self._gripper.open()
return True
#
# Close the gripper
def gripper_close(self):
if self._gripper and self._gripper.is_ready():
if self._is_clicksmart:
if 'grip' in self._gripper.get_ee_signals() :
print(self._gripper_reverse)
self._gripper.set_ee_signal_value('grip', not self._gripper_reverse)
else:
return None
else:
self._gripper.close()
return True
# Grippper vacuum: True:off, False:on
def gripper_vacuum(self, stat=True):
if self._gripper and self._gripper.is_ready():
if self._is_clicksmart:
if 'vacuumOn' in self._gripper.get_ee_signals() :
self._gripper.set_ee_signal_value('vacuumOn', stat)
return self._gripper.get_ee_signal_value('vacuumOn')
return None
def is_gripping(self):
if self._gripper and self._gripper.is_ready():
if self._is_clicksmart:
if self._gripper.get_ee_signal_value('grip') is None:
return not self._gripper.get_ee_signal_value('vacuumOn')
else:
return self._gripper.get_ee_signal_value('grip')
else:
return self._gripper.is_gripping()
return None
###############################################################
#
# stop the thread of velocity control loop
def stop_vctrl(self):
self._running=False
self._vctrl_th.join()
self._vctrl_th=None
#
# start vctrl_loop with Thread
def start_vctrl(self, hz=100):
self._vctrl_th=threading.Thread(target=self.vctrl_loop, args=(hz,self.report,))
self._vctrl_th.start()
#
# velocity control mode, one cycle
def _vctrl_one_cycle(self, func=None):
cmd={}
cur=self._limb.joint_ordered_angles()
dv=np.array(self._target) - np.array(cur)
if np.linalg.norm(dv) < self._accuracy:
if func:
func(self)
self._is_moving=False
else:
self._is_moving=True
vels = map(lambda x: x*self._vrate, dv)
for i,name in enumerate(self._joint_names):
cmd[name]=maxmin(vels[i] , self._velocity_limits[name]*self._vmax, -self._velocity_limits[name]*self._vmax)
self._limb.set_joint_velocities(cmd)
#
# velocity control loop
def vctrl_loop(self, hz, func=None):
rate=rospy.Rate(hz)
self._running=True
while self._running and (not rospy.is_shutdown()) :
cuff_state=self._cuff.cuff_button()
if cuff_state :
self.set_target()
elif self._limb.has_collided() :
self.set_target()
else:
self._vctrl_one_cycle(func)
rate.sleep()
self._limb.exit_control_mode()
print("Terminated")
#
# callback function for Subscriber
def set_target_joint_pos(self, data):
self._target=eval(data.data)
#
#
def set_next_target(self, data):
try:
self.unset_subscriber('current_joint_pos')
next_target=self._target_motion.pop(0)
self.set_target(next_target)
self.set_motion_sequencer()
except:
self.unset_subscriber('current_joint_pos')
pass
#
# Publish current position
def report(self,val):
cur=self._limb.joint_ordered_angles()
self._pub['current_joint_pos'].publish(str(cur))
#
# Set target joint positions (Publish target joint positions)
def set_target(self, val=None, relative=False):
if val is None:
val=self._limb.joint_ordered_angles()
if type(val) is str:
val=self._joint_positions[val]
elif relative:
if len(self._target) != len(val) :
print("Dimension mismatch")
return
for i,v in enumerate(self._target):
val[i]=v + val[i]
self._pub['target_joint_pos'].publish(str(val))
def set_target_seq(self, targets):
self._target_motion=targets
self.set_next_target('star')
def show_positions(self):
print(self._joint_positions.keys())
#
def set_cart_target(self, x,y,z,roll=9,pitch=0,yew=0, in_tip_frame=True):
#pose = self.convert_Cart2Joint(x,y,z, relativa, end_point)
pos=self.calc_cart_move2joints(x,y,z,roll, pitch,yew, in_tip_frame=in_tip_frame)
val=self.joint_pos_d2l(pos)
self._pub['target_joint_pos'].publish(str(val))
#
# Set movement of target joint posistions
def move_joint(self, idxs, vals):
for i,v in enumerate(idxs) :
self._target[v] += vals[i]
self._pub['target_joint_pos'].publish(str(self._target))
#
# end_point should be 'right_hand' or 'right_arm_base_link'.
def convert_Cart2Joint(self, x,y,z, relative=False, end_point='right_hand'):
_pose=self.endpoint_pose()
if relative:
_pose.position.x += x
_pose.position.y += y
_pose.position.z += z
else:
_pose.position.x = x
_pose.position.y = y
_pose.position.z = z
return self._limb.ik_request(_pose, end_point=end_point)
#
#
def convert_Joint2Cart(self, pos):
_pos=self._limb.joint_angles()
for i,name in enumerate(self._joint_names):
_pos[name]=pos[i]
return self._limb.joint_angles_to_cartesian_pose(_pos)
def endpoint_pose(self):
return self._limb.tip_state('right_hand').pose
#return self._limb.joint_angles_to_cartesian_pose(self._limb.joint_angles())
def calc_relative_pose(self, x,y,z,roll=0,pitch=0,yew=0, in_tip_frame=True):
_pose=self.endpoint_pose()
########################################
# set target position
trans = PyKDL.Vector(x,y,z)
rot = PyKDL.Rotation.RPY(roll,pitch,yew)
f2 = PyKDL.Frame(rot, trans)
if in_tip_frame:
# endpoint's cartesian systems
_pose=posemath.toMsg(posemath.fromMsg(_pose) * f2)
else:
# base's cartesian systems
_pose=posemath.toMsg(f2 * posemath.fromMsg(_pose))
return _pose
def calc_cart_move2joints(self, x,y,z,roll=0,pitch=0,yew=0, in_tip_frame=True):
_pose=self.calc_relative_pose(x,y,z,roll,pitch,yew, in_tip_frame)
return self._limb.ik_request(_pose)
########################################################################
#
# onExecuted
#
def onExecute(self):
cuff_state=self._cuff.cuff_button()
if self._is_pause :
self._limb.set_joint_velocities(self._stop_cmd)
elif cuff_state :
self.set_target()
elif self._limb.has_collided() :
self.set_target()
else:
self._vctrl_one_cycle(self.report)
#
# for RTC(Common)
#
def clearAlarms(self):
print('No alerm..')
return True
def getActiveAlarm(self):
res=[]
return res
def getFeedbackPosJoint(self):
res=map(lambda x: np.rad2deg(x), self._limb.joint_ordered_angles())
res.append(self.gripper_state())
return res
# manifactur: string
# type: string
# axisNum: ULONG
# cmdCycle: ULONG
# isGripper: boolea
def getManipInfo(self):
if self._gripper :
res=['RethinkRobotics', 'Sawyer', 7, 100, True]
else:
res=['RethinkRobotics', 'Sawyer', 7, 100, False]
return res
def getSoftLimitJoint(self):
return None
def getState(self):
stat=self._rs.state()
if stat.enabled :
res=0x01
if self._is_moveing :
res=0x01 | 0x02
else:
res=0x00
return res
def servoOFF(self):
return None
def servoON(self):
return None
def setSoftLimitJoint(self, softLimit):
return None
#
# for RTC(Middle)
#
def openGripper(self):
if self._gripper_type == 'vacuum':
self.gripper_vacuum(False)
else:
self.gripper_open()
return True
def closeGripper(self):
if self._gripper_type == 'vacuum':
self.gripper_vacuum(True)
else:
self.gripper_close()
return True
def moveGripper(self, angleRatio):
print('Move gripper is not supported')
return None
def getBaseOffset(self):
return None
def getFeedbackPosCartesian(self):
return None
def getMaxSpeedCartesian(self):
return None
def getMaxSpeedJoint(self):
return None
def getMinAccelTimeCartesian(self):
return None
def getMinAccelTimeJoint(self):
return None
def getSoftLimitCartesian(self):
return None
def moveLinearCartesianAbs(self, carPos, elbow, flag):
return None
def moveLinearCartesianRel(self, carPos, elbow, flag):
return None
def movePTPCartesianAbs(self, carPos, elbow, flag):
mx=np.array(carPos)
posm=map(lambda x: x/1000.0, mx[:,3])
mx=np.vstack((mx, [0,0,0,1]))
qtn=tf.transformations.quaternion_from_matrix(mx)
pose=newPose(posm, qtn)
angles=self._limb.ik_request(pose)
self._target=self.joint_pos_d2l(angles)
return True
def movePTPCartesianRel(self, carPos, elbow, flag):
ep=self.endpoint_pose()
mx=np.array(carPos)
p=map(lambda x: x/1000.0, mx[:,3])
mx=np.vstack((mx, [0,0,0,1]))
el = tf.transformations.euler_from_matrix(mx)
pos=self.calc_cart_move2joints(p[0],p[1],p[2],el[0],el[1],el[2], (flag==1))
self._target=self.joint_pos_d2l(pos)
return True
def movePTPJointAbs(self, jointPoints):
if len(jointPoints) >= 7:
pos=map(lambda x: np.deg2rad(x), jointPoints[:7])
self._target=pos
return True
else:
return False
def movePTPJointRel(self, jointPoints):
if len(jointPoints) >= 7:
pos=map(lambda x: np.deg2rad(x), jointPoints[:7])
self._target=np.array(self._target) + np.array(pos)
return True
else:
return False
def pause_motion(self):
self._is_pause=True
self._limb.set_joint_velocities(self._stop_cmd)
return True
def resume_motion(self):
self._is_pause=False
return True
def stop_motion(self):
self.set_target()
return True
def setAccelTimeCartesian(self, aclTime):
return None
def setAccelTimeJoint(self, aclTime):
return None
def setBaseOffset(self, offset):
return None
def setControlPointOffset(self, offset):
return None
def setMaxSpeedCartesian(self, sp_trans, sp_rot):
return None
def setMaxSpeedJoint(self, speed):
return None
def setMinAccelTimeCartesian(self, aclTime):
return None
def setMinAccelTimeJoint(self, aclTime):
return None
def setSoftLimitCartesian(self, xLimit, yLimit, zLimit):
return None
def setSpeedCartesian(self, spdRatio):
return None
def setSpeedJoint(self, spdRatio):
self.set_speed(spdRatio)
return True
def moveCircularCartesianAbs(self, carPointR, carPointT):
return None
def moveCircularCartesianRel(self, carPointR, carPointT):
return None
def setHome(self, jointPoint):
self._home_pos=jointPoint
return True
def getHome(self):
return self._home_pos
def goHome(self):
self._target=self._home_pos
return True
#
#
def moveCartesianRel(self, pos, rot, flag):
p=map(lambda x: x/1000.0, pos)
r=map(lambda x: np.deg2rad(x), rot)
self.set_cart_target(p[0], p[1], p[2], r[0], r[1], r[2], flag)
return True
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():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
#####
parser.add_argument(
"-p", "--position", type=float,
nargs='+', default=[0, 0, 0],
help="Desired end position: X, Y, Z")
parser.add_argument(
"-o", "--orientation", type=float,
nargs='+',
default=[0.704020578925, 0.710172716916, 0.00244101361829, 0.00194372088834],
help="Orientation as a quaternion (x, y, z, w)")
#####
parser.add_argument(
"-q", "--joint_angles", type=float,
nargs='+', default=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0],
help="A list of joint angles, one for each of the 7 joints, J0...J6")
parser.add_argument(
"-s", "--speed_ratio", type=float, default=0.2,
help="A value between 0.001 (slow) and 1.0 (maximum joint velocity)")
parser.add_argument(
"-a", "--accel_ratio", type=float, default=0.05,
help="A value between 0.001 (slow) and 1.0 (maximum joint accel)")
parser.add_argument(
"-t", "--trajType", type=str, default='JOINT',
choices=['JOINT', 'CARTESIAN'],
help="trajectory interpolation type")
parser.add_argument(
"-st", "--interaction_active", type=int, default=1, choices = [0, 1],
help="Activate (1) or Deactivate (0) interaction controller")
parser.add_argument(
"-k", "--K_impedance", type=float,
nargs='+', default=[1300.0, 1300.0, 1300.0, 30.0, 30.0, 30.0],
help="A list of desired stiffnesses, one for each of the 6 directions -- stiffness units are (N/m) for first 3 and (Nm/rad) for second 3 values")
parser.add_argument(
"-m", "--max_impedance", type=int,
nargs='+', default=[1, 1, 1, 1, 1, 1], choices = [0, 1],
help="A list of impedance modulation state, one for each of the 6 directions (a single value can be provided to apply the same value to all the directions) -- 0 for False, 1 for True")
parser.add_argument(
"-md", "--interaction_control_mode", type=int,
nargs='+', default=[1, 1, 1, 1, 1, 1], choices = [1,2,3,4],
help="A list of desired interaction control mode (1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit), one for each of the 6 directions")
parser.add_argument(
"-fr", "--interaction_frame", type=float,
nargs='+', default=[0, 0, 0, 1, 0, 0, 0],
help="Specify the reference frame for the interaction controller -- first 3 values are positions [m] and last 4 values are orientation in quaternion (w, x, y, z)")
parser.add_argument(
"-ef", "--in_endpoint_frame", action='store_true', default=False,
help="Set the desired reference frame to endpoint frame; otherwise, it is base frame by default")
parser.add_argument(
"-en", "--endpoint_name", type=str, default='right_hand',
help="Set the desired endpoint frame by its name; otherwise, it is right_hand frame by default")
parser.add_argument(
"-f", "--force_command", type=float,
nargs='+', default=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
help="A list of desired force commands, one for each of the 6 directions -- in force control mode this is the vector of desired forces/torques to be regulated in (N) and (Nm), in impedance with force limit mode this vector specifies the magnitude of forces/torques (N and Nm) that the command will not exceed")
parser.add_argument(
"-kn", "--K_nullspace", type=float,
nargs='+', default=[5.0, 10.0, 5.0, 10.0, 5.0, 10.0, 5.0],
help="A list of desired nullspace stiffnesses, one for each of the 7 joints (a single value can be provided to apply the same value to all the directions) -- units are in (Nm/rad)")
parser.add_argument(
"-dd", "--disable_damping_in_force_control", action='store_true', default=False,
help="Disable damping in force control")
parser.add_argument(
"-dr", "--disable_reference_resetting", action='store_true', default=False,
help="The reference signal is reset to actual position to avoid jerks/jumps when interaction parameters are changed. This option allows the user to disable this feature.")
parser.add_argument(
"-rc", "--rotations_for_constrained_zeroG", action='store_true', default=False,
help="Allow arbitrary rotational displacements from the current orientation for constrained zero-G (use only for a stationary reference orientation)")
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(rospy.myargv()[1:])
try:
rospy.init_node('path_planner_py')
limb = Limb()
traj = MotionTrajectory(limb = limb)
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)
# joint_angles = limb.joint_ordered_angles()
# waypoint.set_joint_angles(joint_angles = joint_angles)
# traj.append_waypoint(waypoint.to_msg())
# joint = ik_service_client(poses).values()[::-1] # joint angles from J0 to J6
# if len(joint_angles) != len(joint_angles):
# rospy.logerr('The number of joint_angles must be %d', len(joint_angles))
# return None
# # waypoint.set_joint_angles(joint_angles = args.joint_angles)
# waypoint.set_joint_angles(joint_angles = joint)
#####divide the whole path into three parts: soft begin, uniform motion, soft stop####
final_pos = args.position
# get endpoint state
endpoint_state = limb.tip_state('right_hand')
current_pos = endpoint_state.pose.position
dis = [final_pos[0]-current_pos.x, final_pos[1]-current_pos.y, final_pos[2]-current_pos.z]
uniform_motion = [current_pos.x + dis[0]/5, current_pos.y + dis[1]/5, current_pos.z + dis[2]/5]
soft_stop = [current_pos.x + 4*dis[0]/5, current_pos.y + 4*dis[1]/5, current_pos.z + 4*dis[2]/5]
#######################################################################################
# waypoint = path_planning(uniform_motion, args.orientation, 0.25, 0.01)
# traj.append_waypoint(waypoint.to_msg())
# waypoint = path_planning(soft_stop, args.orientation, 0.25, 0)
# traj.append_waypoint(waypoint.to_msg())
# waypoint = path_planning(final_pos, args.orientation, 0.25, 0.01)
# # joint = path_planning(uniform_motion, args.orientation, 0.2, 0.1) # joint angles from J0 to J6
# # waypoint.set_joint_angles(joint_angles = joint)
# # traj.append_waypoint(waypoint.to_msg())
# # joint = path_planning(soft_stop, args.orientation, 0.2, 0.1) # joint angles from J0 to J6
# # waypoint.set_joint_angles(joint_angles = joint)
# # traj.append_waypoint(waypoint.to_msg())
# # joint = path_planning(final_pos, args.orientation, 0.2, 0.1) # joint angles from J0 to J6
# # waypoint.set_joint_angles(joint_angles = joint)
# traj.append_waypoint(waypoint.to_msg())
###########open traj file
filename = 'traj'
with open(filename, 'r') as f:
lines = f.readlines()
l = len(lines) - 1
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.5,
max_joint_accel=0.01)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
for line in lines[1:int(floor(2*l/5))]:
print(line)
jnt_angles = [float(x) for x in line.rstrip().split(',')[1:8]]
waypoint.set_joint_angles(joint_angles = jnt_angles)
traj.append_waypoint(waypoint.to_msg())
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.5,
max_joint_accel=0)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
for line in lines[int(floor(2*l/5)):int(floor(3*l/5))]:
print(line)
jnt_angles = [float(x) for x in line.rstrip().split(',')[1:8]]
waypoint.set_joint_angles(joint_angles = jnt_angles)
traj.append_waypoint(waypoint.to_msg())
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.5,
max_joint_accel=0.01)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
for line in lines[int(floor(3*l/5)):]:
print(line)
jnt_angles = [float(x) for x in line.rstrip().split(',')[1:8]]
waypoint.set_joint_angles(joint_angles = jnt_angles)
traj.append_waypoint(waypoint.to_msg())
# set the interaction control options in the current configuration
interaction_options = InteractionOptions()
trajectory_options = TrajectoryOptions()
trajectory_options.interaction_control = True
trajectory_options.interpolation_type = args.trajType
interaction_options.set_interaction_control_active(int2bool(args.interaction_active))
interaction_options.set_K_impedance(args.K_impedance)
interaction_options.set_max_impedance(int2bool(args.max_impedance))
interaction_options.set_interaction_control_mode(args.interaction_control_mode)
interaction_options.set_in_endpoint_frame(int2bool(args.in_endpoint_frame))
interaction_options.set_force_command(args.force_command)
interaction_options.set_K_nullspace(args.K_nullspace)
interaction_options.set_endpoint_name(args.endpoint_name)
if len(args.interaction_frame) < 7:
rospy.logerr('The number of elements must be 7!')
elif len(args.interaction_frame) == 7:
quat_sum_square = args.interaction_frame[3]*args.interaction_frame[3] + args.interaction_frame[4]*args.interaction_frame[4]
+ args.interaction_frame[5]*args.interaction_frame[5] + args.interaction_frame[6]*args.interaction_frame[6]
if quat_sum_square < 1.0 + 1e-7 and quat_sum_square > 1.0 - 1e-7:
interaction_frame = Pose()
interaction_frame.position.x = args.interaction_frame[0]
interaction_frame.position.y = args.interaction_frame[1]
interaction_frame.position.z = args.interaction_frame[2]
interaction_frame.orientation.w = args.interaction_frame[3]
interaction_frame.orientation.x = args.interaction_frame[4]
interaction_frame.orientation.y = args.interaction_frame[5]
interaction_frame.orientation.z = args.interaction_frame[6]
interaction_options.set_interaction_frame(interaction_frame)
else:
rospy.logerr('Invalid input to quaternion! The quaternion must be a unit quaternion!')
else:
rospy.logerr('Invalid input to interaction_frame!')
interaction_options.set_disable_damping_in_force_control(args.disable_damping_in_force_control)
interaction_options.set_disable_reference_resetting(args.disable_reference_resetting)
interaction_options.set_rotations_for_constrained_zeroG(args.rotations_for_constrained_zeroG)
trajectory_options.interaction_params = interaction_options.to_msg()
traj.set_trajectory_options(trajectory_options)
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 with interaction options set!')
else:
rospy.logerr('Motion controller failed to complete the trajectory with error %s',
result.errorId)
# print the resultant interaction options
rospy.loginfo('Interaction Options:\n%s', interaction_options.to_msg())
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user. %s',
'Exiting before trajectory completion.')
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.')
class Breath(object):
def __init__(self):
self.robot_state = 0 # normal
self.breath_state =0 # false
rospy.Subscriber("cs_sawyer/head_light", UInt8, self.callback_update_breath1)
rospy.Subscriber("cs_sawyer/breath", Bool, self.callback_update_breath2)
self.rospack = rospkg.RosPack()
# Set the trajectory options
self.limb = Limb()
traj_opts = TrajectoryOptions()
traj_opts.interpolation_type = 'JOINT'
self.traj = MotionTrajectory(trajectory_options = traj_opts, limb = self.limb)
# Set the waypoint options
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.05, joint_tolerances=0.7)
#max_joint_accel=0.1)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = self.limb)
# Append a waypoint at the current pose
waypoint.set_joint_angles(self.limb.joint_ordered_angles())
#self.traj.append_waypoint(waypoint.to_msg())
#self.limb.set_joint_position_speed(0.3)
with open(join(self.rospack.get_path("cs_sawyer"), "config/poses.json")) as f:
self.poses = json.load(f)
joint_angles= [self.poses["pause"][j] for j in [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6']]
j1= joint_angles[1]
j2= joint_angles[2]
x=0
while x < 16*pi:
new_j1 = 0.07*sin(x)+j1
new_j2=0.09*sin(0.5*x)+j2
joint_angles[1]=new_j1
joint_angles[2]=new_j2
x=x+pi/40
waypoint.set_joint_angles(joint_angles = joint_angles)
self.traj.append_waypoint(waypoint.to_msg())
def callback_update_breath1(self,msg):
self.robot_state = msg.data
if self.robot_state != 0:
self.traj.stop_trajectory()
def callback_update_breath2(self,msg):
self.breath_state = msg.data
if not self.breath_state:
self.traj.stop_trajectory()
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if self.breath_state and self.robot_state == 0:
self.traj.send_trajectory(timeout=None)
rate.sleep()
def run(self):
rate = rospy.Rate(100)
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.5,
max_joint_accel=0.5,
corner_distance=0.05)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
self.pose.header = Header(stamp=rospy.Time.now(), frame_id='base')
self.pose.pose.position.x = 0.0
self.pose.pose.position.y = -0.6
self.pose.pose.position.z = 0.5
self.pose.pose.orientation.x = 0.5
self.pose.pose.orientation.y = -0.5
self.pose.pose.orientation.z = 0.5
self.pose.pose.orientation.w = 0.5
joint_angles = limb.joint_ordered_angles()
waypoint.set_cartesian_pose(self.pose, "right_hand", joint_angles)
self.waypoints.append(waypoint)
rospy.loginfo("Sending inital waypoint: %s",
self.waypoints[0].to_string())
traj.append_waypoint(self.waypoints[0].to_msg())
result = traj.send_trajectory()
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)
traj.clear_waypoints()
l = Lights()
l_name = 'head_green_light'
initial_state = l.get_light_state(l_name)
for i in range(0, 2):
state = not initial_state
l.set_light_state(l_name, state)
rospy.sleep(0.5)
state = not state
l.set_light_state(l_name, state)
rospy.sleep(0.5)
l.set_light_state(l_name, True)
for i in range(0, 19):
self.gen_rand_waypoint()
sendCommand = True
while not rospy.is_shutdown():
traj.clear_waypoints()
for i in range(0, 19):
self.waypoints.pop(i)
self.gen_rand_waypoint()
for point in self.waypoints:
traj.append_waypoint(point.to_msg())
print(len(self.waypoints))
result = traj.send_trajectory(wait_for_result=True)
self.firstShutdown = True
def clean_shutdown():
if self.firstShutdown:
print("STOPPING TRAJECTORY")
traj.stop_trajectory()
traj.clear_waypoints()
l = Lights()
l.set_light_state('head_green_light', False)
self.firstShutdown = False
rospy.on_shutdown(clean_shutdown)
rate.sleep()
return