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 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 cartesian_pose(args):
"""
Move the robot arm to the specified configuration.
Call using:
$ rosrun intera_examples go_to_cartesian_pose.py [arguments: see below]
-p 0.4 -0.3 0.18 -o 0.0 1.0 0.0 0.0 -t right_hand
--> Go to position: x=0.4, y=-0.3, z=0.18 meters
--> with quaternion orientation (0, 1, 0, 0) and tip name right_hand
--> The current position or orientation will be used if only one is provided.
-q 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0
--> Go to joint angles: 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 using default settings
--> If a Cartesian pose is not provided, Forward kinematics will be used
--> If a Cartesian pose is provided, the joint angles will be used to bias the nullspace
-R 0.01 0.02 0.03 0.1 0.2 0.3 -T
-> Jog arm with Relative Pose (in tip frame)
-> x=0.01, y=0.02, z=0.03 meters, roll=0.1, pitch=0.2, yaw=0.3 radians
-> The fixed position and orientation paramters will be ignored if provided
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description="cartesian_pose.__doc__")
parser.add_argument(
"-p", "--position", type=float,
nargs='+',
help="Desired end position: X, Y, Z")
parser.add_argument(
"-o", "--orientation", type=float,
nargs='+',
help="Orientation as a quaternion (x, y, z, w)")
parser.add_argument(
"-R", "--relative_pose", type=float,
nargs='+',
help="Jog pose by a relative amount in the base frame: X, Y, Z, roll, pitch, yaw")
parser.add_argument(
"-T", "--in_tip_frame", action='store_true',
help="For relative jogs, job in tip frame (default is base frame)")
parser.add_argument(
"-q", "--joint_angles", type=float,
nargs='+', default=[],
help="A list of joint angles, one for each of the 7 joints, J0...J6")
parser.add_argument(
"-t", "--tip_name", default='right_hand',
help="The tip name used by the Cartesian pose")
parser.add_argument(
"--linear_speed", type=float, default=0.6,
help="The max linear speed of the endpoint (m/s)")
parser.add_argument(
"--linear_accel", type=float, default=0.6,
help="The max linear acceleration of the endpoint (m/s/s)")
parser.add_argument(
"--rotational_speed", type=float, default=1.57,
help="The max rotational speed of the endpoint (rad/s)")
parser.add_argument(
"--rotational_accel", type=float, default=1.57,
help="The max rotational acceleration of the endpoint (rad/s/s)")
parser.add_argument(
"--timeout", type=float, default=None,
help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.")
"""
try:
#rospy.init_node('go_to_cartesian_pose_py')
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions(
max_linear_speed=args.linear_speed,
max_linear_accel=args.linear_accel,
max_rotational_speed=args.rotational_speed,
max_rotational_accel=args.rotational_accel,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_names = limb.joint_names()
if args.joint_angles and len(args.joint_angles) != len(joint_names):
rospy.logerr('len(joint_angles) does not match len(joint_names!)')
return None
if (args.position is None and args.orientation is None
and args.relative_pose is None):
if args.joint_angles:
# does Forward Kinematics
waypoint.set_joint_angles(args.joint_angles, args.tip_name,
joint_names)
else:
rospy.loginfo(
"No Cartesian pose or joint angles given. Using default")
waypoint.set_joint_angles(joint_angles=None,
active_endpoint=args.tip_name)
else:
endpoint_state = limb.tip_state(args.tip_name)
if endpoint_state is None:
rospy.logerr('Endpoint state not found with tip name %s',
args.tip_name)
return None
pose = endpoint_state.pose
if args.relative_pose is not None:
if len(args.relative_pose) != 6:
rospy.logerr(
'Relative pose needs to have 6 elements (x,y,z,roll,pitch,yaw)'
)
return None
# create kdl frame from relative pose
rot = PyKDL.Rotation.RPY(args.relative_pose[3],
args.relative_pose[4],
args.relative_pose[5])
trans = PyKDL.Vector(args.relative_pose[0],
args.relative_pose[1],
args.relative_pose[2])
f2 = PyKDL.Frame(rot, trans)
# and convert the result back to a pose message
if args.in_tip_frame:
# end effector frame
pose = posemath.toMsg(posemath.fromMsg(pose) * f2)
else:
# base frame
pose = posemath.toMsg(f2 * posemath.fromMsg(pose))
else:
if args.position is not None and len(args.position) == 3:
pose.position.x = args.position[0]
pose.position.y = args.position[1]
pose.position.z = args.position[2]
if args.orientation is not None and len(args.orientation) == 4:
pose.orientation.x = args.orientation[0]
pose.orientation.y = args.orientation[1]
pose.orientation.z = args.orientation[2]
pose.orientation.w = args.orientation[3]
poseStamped = PoseStamped()
poseStamped.pose = pose
waypoint.set_cartesian_pose(poseStamped, args.tip_name,
args.joint_angles)
rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string())
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=args.timeout)
if result is None:
rospy.logerr('Trajectory FAILED to send')
return
if result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!')
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
def go_to_cartesian(position=None,
orientation=None,
joint_angles=None,
linear_speed=0.1,
rotational_speed=0.57,
linear_accel=0.3,
tip_name='right_hand',
relative_pose=None,
rotational_accel=0.57,
timeout=None):
if not rospy.is_shutdown():
try:
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions(
max_linear_speed=linear_speed,
max_linear_accel=linear_accel,
max_rotational_speed=rotational_speed,
max_rotational_accel=rotational_accel,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
joint_names = limb.joint_names()
if joint_angles and len(joint_angles) != len(joint_names):
rospy.logerr(
'len(joint_angles) does not match len(joint_names!)')
return None
if (position is None and orientation is None
and relative_pose is None):
if joint_angles:
# does Forward Kinematics
waypoint.set_joint_angles(joint_angles, tip_name,
joint_names)
else:
rospy.loginfo(
"No Cartesian pose or joint angles given. Using default"
)
waypoint.set_joint_angles(joint_angles=None,
active_endpoint=tip_name)
else:
endpoint_state = limb.tip_state(tip_name)
if endpoint_state is None:
rospy.logerr('Endpoint state not found with tip name %s',
tip_name)
return None
pose = endpoint_state.pose
if relative_pose is not None:
if len(relative_pose) != 6:
rospy.logerr(
'Relative pose needs to have 6 elements (x,y,z,roll,pitch,yaw)'
)
return None
# create kdl frame from relative pose
rot = PyKDL.Rotation.RPY(relative_pose[3],
relative_pose[4],
relative_pose[5])
trans = PyKDL.Vector(relative_pose[0], relative_pose[1],
relative_pose[2])
f2 = PyKDL.Frame(rot, trans)
# and convert the result back to a pose message
if in_tip_frame:
# end effector frame
pose = posemath.toMsg(posemath.fromMsg(pose) * f2)
else:
# base frame
pose = posemath.toMsg(f2 * posemath.fromMsg(pose))
else:
if position is not None:
pose.position.x = position.x
pose.position.y = position.y
pose.position.z = position.z
if orientation is not None:
pose.orientation.x = orientation.x
pose.orientation.y = orientation.y
pose.orientation.z = orientation.z
pose.orientation.w = orientation.w
poseStamped = PoseStamped()
poseStamped.pose = pose
waypoint.set_cartesian_pose(poseStamped, tip_name,
joint_angles)
#rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string())
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=timeout)
if result is None:
return
if result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!')
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr(
'Keyboard interrupt detected from the user. Exiting before trajectory completion.'
)
def move2cartesian(position=None, orientation=None, relative_pose=None, in_tip_frame=False, joint_angles=[],
tip_name='right_hand', linear_speed=0.6, linear_accel=0.6, rotational_speed=1.57,
rotational_accel=1.57, timeout=None, neutral=False):
"""
Move the robot arm to the specified configuration.
Call using:
$ rosrun intera_examples go_to_cartesian_pose.py [arguments: see below]
-p 0.4 -0.3 0.18 -o 0.0 1.0 0.0 0.0 -t right_hand
--> Go to position: x=0.4, y=-0.3, z=0.18 meters
--> with quaternion orientation (0, 1, 0, 0) and tip name right_hand
--> The current position or orientation will be used if only one is provided.
-q 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0
--> Go to joint angles: 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 using default settings
--> If a Cartesian pose is not provided, Forward kinematics will be used
--> If a Cartesian pose is provided, the joint angles will be used to bias the nullspace
-R 0.01 0.02 0.03 0.1 0.2 0.3 -T
-> Jog arm with Relative Pose (in tip frame)
-> x=0.01, y=0.02, z=0.03 meters, roll=0.1, pitch=0.2, yaw=0.3 radians
-> The fixed position and orientation paramters will be ignored if provided
"""
try:
#rospy.init_node('go_to_cartesian_pose_py')
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options = traj_options, limb = limb)
wpt_opts = MotionWaypointOptions(max_linear_speed=linear_speed,
max_linear_accel=linear_accel,
max_rotational_speed=rotational_speed,
max_rotational_accel=rotational_accel,
max_joint_speed_ratio=1.0)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
joint_names = limb.joint_names()
if joint_angles and len(joint_angles) != len(joint_names):
rospy.logerr('len(joint_angles) does not match len(joint_names!)')
return None
if neutral == True:
limb.move_to_neutral()
else:
if (position is None and orientation is None and relative_pose is None):
if joint_angles:
# does Forward Kinematics
waypoint.set_joint_angles(joint_angles, tip_name, joint_names)
else:
rospy.loginfo("No Cartesian pose or joint angles given. Using default")
waypoint.set_joint_angles(joint_angles=None, active_endpoint=tip_name)
else:
endpoint_state = limb.tip_state(tip_name)
if endpoint_state is None:
rospy.logerr('Endpoint state not found with tip name %s', tip_name)
return None
pose = endpoint_state.pose
if relative_pose is not None:
if len(relative_pose) != 6:
rospy.logerr('Relative pose needs to have 6 elements (x,y,z,roll,pitch,yaw)')
return None
# create kdl frame from relative pose
rot = PyKDL.Rotation.RPY(relative_pose[3],
relative_pose[4],
relative_pose[5])
trans = PyKDL.Vector(relative_pose[0],
relative_pose[1],
relative_pose[2])
f2 = PyKDL.Frame(rot, trans)
# and convert the result back to a pose message
if in_tip_frame:
# end effector frame
pose = posemath.toMsg(posemath.fromMsg(pose) * f2)
else:
# base frame
pose = posemath.toMsg(f2 * posemath.fromMsg(pose))
else:
if position is not None and len(position) == 3:
pose.position.x = position[0]
pose.position.y = position[1]
pose.position.z = position[2]
if orientation is not None and len(orientation) == 4:
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[3]
poseStamped = PoseStamped()
poseStamped.pose = pose
if not joint_angles:
# using current joint angles for nullspace bais if not provided
joint_angles = limb.joint_ordered_angles()
waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles)
else:
waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles)
rospy.loginfo('Sending waypoint: \n%s', waypoint.to_string())
traj.append_waypoint(waypoint.to_msg())
result = traj.send_trajectory(timeout=timeout)
if result is None:
rospy.logerr('Trajectory FAILED to send')
return
if result.result:
rospy.loginfo('Motion controller successfully finished the trajectory!')
else:
rospy.logerr('Motion controller failed to complete the trajectory with error %s',
result.errorId)
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user. Exiting before trajectory completion.')
def 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 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"
class CircularMotion():
def __init__(self):
self.mover = move_to_point.MoveToPoint()
self.retrieve_height = -0.015
self._limb = Limb()
self.traj_options = TrajectoryOptions()
self.traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
self.traj = MotionTrajectory(trajectory_options=self.traj_options,
limb=self._limb)
self.wpt_options = MotionWaypointOptions(max_linear_speed=0.4,
max_linear_accel=0.4,
max_rotational_speed=1.57,
max_rotational_accel=1.57,
max_joint_speed_ratio=1.0)
self.waypoint = MotionWaypoint(options=self.wpt_options.to_msg(),
limb=self._limb)
self.joint_names = self._limb.joint_names()
endpoint_state = self._limb.tip_state('right_hand')
self.pose = endpoint_state.pose
def to_quaternion(self, roll, pitch, yaw):
cy = math.cos(0.5 * yaw)
sy = math.sin(0.5 * yaw)
cr = math.cos(0.5 * roll)
sr = math.sin(0.5 * roll)
cp = math.cos(0.5 * pitch)
sp = math.sin(0.5 * pitch)
qx = cy * sr * cp - sy * cr * sp
qy = cy * cr * sp + sy * sr * cp
qz = sy * cr * cp - cy * sr * sp
qw = cy * cr * cp + sy * sr * sp
orientation = [qx, qy, qz, qw]
return orientation
def displacement(self, time):
disp = 0.1667 * math.pi * math.sin(time)
return disp
def move(self, cur_pos):
curr_roll = 1.1667 * math.pi
curr_pitch = 0
curr_yaw = 0
self.mover.move(cur_pos[0], cur_pos[1], self.retrieve_height,
cur_pos[3], cur_pos[4], cur_pos[5], cur_pos[6])
rospy.sleep(1)
time = 0
while time < 2 * math.pi:
end = self.to_quaternion(curr_roll, curr_pitch, curr_yaw)
self.pose.position.x = cur_pos[0]
self.pose.position.y = cur_pos[1]
self.pose.position.z = self.retrieve_height
self.pose.orientation.x = end[0]
self.pose.orientation.y = end[1]
self.pose.orientation.z = end[2]
self.pose.orientation.w = end[3]
if time < 0.5 * math.pi:
curr_roll = 1.1667 * math.pi - self.displacement(time)
elif time < math.pi:
curr_roll = 0.833 * math.pi + self.displacement(time)
elif time < 1.5 * math.pi:
curr_roll = 0.833 * math.pi - self.displacement(time)
else:
curr_roll = 1.1667 * math.pi + self.displacement(time)
curr_pitch = self.displacement(time)
time = time + 0.1
poseStamped = PoseStamped()
poseStamped.pose = self.pose
self.waypoint.set_cartesian_pose(poseStamped, 'right_hand', [])
self.traj.append_waypoint(self.waypoint.to_msg())
result = self.traj.send_trajectory(timeout=None)
