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 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 main():
"""
Set the desired interaction control options in the current configuration.
Note that the arm is not commanded to move but it will have the specified
interaction control behavior. If publish rate is 0 where the interaction
control command is only published once, after entering and exiting zero-G,
the arm will return to normal position mode. Also, regardless of the publish
rate, the zero-G behavior will not be affected by this. The future motion
commands need to be sent with interaction parameters if we want to keep
interaction control behaviors during the trajectory execution; otherwise,
the arm will move in position mode during the motion even if this script
is still running.
Call using:
$ rosrun intera_examples set_interaction_options.py [arguments: see below]
-s 1
--> Set interaction active to 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 1 1 0 1 1 1
--> Set max_impedance to [True True False True True True] in the current configuration
-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 w/ force limit,
4: force w/ motion limit)
-fr 0.1 0.2 0.3 1 0 0 0
--> Set the pose of the interaction_frame -- position: (0.1, 0.2, 0.3) and orientation (1, 0, 0, 0)
-ef
--> Set in_endpoint_frame to True in the current configuration (use TCP frame as reference frame)
-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
-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
-r 20
--> Set desired publish rate (Hz)
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument(
"-s",
"--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 maximum stiffness behavior 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) which has to be normalized values"
)
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 (works only with a stationary reference orientation)"
)
parser.add_argument(
"-r",
"--rate",
type=int,
default=10,
help=
"A desired publish rate for updating interaction control commands (10Hz by default) -- 0 if we want to publish it only once"
)
args = parser.parse_args(rospy.myargv()[1:])
try:
rospy.init_node('set_interaction_options_py')
pub = rospy.Publisher('/robot/limb/right/interaction_control_command',
InteractionControlCommand,
queue_size=1)
rospy.sleep(0.5)
if args.rate > 0:
rate = rospy.Rate(args.rate)
elif args.rate == 0:
rospy.logwarn('Interaction control options will be set only once!')
elif args.rate < 0:
rospy.logerr('Invalid publish rate!')
# set the interaction control options in the current configuration
interaction_options = InteractionOptions()
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(args.in_endpoint_frame)
interaction_options.set_force_command(args.force_command)
interaction_options.set_K_nullspace(args.K_nullspace)
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)
msg = interaction_options.to_msg()
# print the resultant interaction options once
rospy.loginfo(msg)
pub.publish(msg)
rs = intera_interface.RobotEnable(CHECK_VERSION)
if args.rate > 0:
while not rospy.is_shutdown() and rs.state().enabled:
rate.sleep()
pub.publish(msg)
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user. %s',
'Exiting the node...')
if not rs.state().enabled:
# send a message to put the robot back into position mode
position_mode = InteractionOptions()
position_mode.set_interaction_control_active(False)
pub.publish(position_mode.to_msg())
rospy.sleep(0.5)
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 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 main():
"""
Set the desired interaction control options in the current configuration.
Note that the arm is not commanded to move but it will have the specified
interaction control behavior.
If publish the rate is 0, the interaction control command is only published
once; otherwise a last position command will be sent when the script exits.
For non-zero publishing rates, the arm will go back into constrained zero-G
if the arm's zero-g button is pressed and relased. Future motion commands
will use the interaction parameters set in the trajectory options.
Call using:
$ rosrun intera_examples set_interaction_options.py [arguments: see below]
-s 1
--> Set interaction active to 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 1 1 0 1 1 1
--> Set max_impedance to [True True False True True True] in the current configuration
-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 w/ force limit,
4: force w/ motion limit)
-fr 0.1 0.2 0.3 1 0 0 0
--> Set the pose of the interaction_frame -- position: (0.1, 0.2, 0.3) and orientation (1, 0, 0, 0)
-ef
--> Set in_endpoint_frame to True in the current configuration (use TCP frame as reference frame)
-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
-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
-r 20
--> Set desired publish rate (Hz)
-r 0
--> The interaction command is published once and exits. The arm can remain
in interaction control after this script.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument(
"-s",
"--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 maximum stiffness behavior 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) which has to be normalized values"
)
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 (works only with a stationary reference orientation)"
)
parser.add_argument(
"-r",
"--rate",
type=int,
default=10,
help=
"A desired publish rate for updating interaction control commands (10Hz by default) -- a rate 0 publish once and exits which can cause the arm to remain in interaction control."
)
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('set_interaction_options')
# set the interaction control options in the current configuration
interaction_options = InteractionOptions()
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(args.in_endpoint_frame)
interaction_options.set_force_command(args.force_command)
interaction_options.set_K_nullspace(args.K_nullspace)
if len(args.interaction_frame) == 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 interaction_frame. Must be 7 elements.')
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)
# print the resultant interaction options once
rospy.loginfo(interaction_options.to_msg())
ic_pub = InteractionPublisher()
rospy.sleep(0.5)
if args.rate != 0:
rospy.on_shutdown(ic_pub.send_position_mode_cmd)
ic_pub.send_command(interaction_options, args.rate)
if args.rate == 0:
rospy.sleep(0.5)