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 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_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 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 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 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"