def position_mode(self):
# send a message to put the robot back into position mode
rospy.loginfo('Entering position mode')
position_mode = InteractionOptions()
position_mode.set_interaction_control_active(False)
self.icc_pub.publish(position_mode.to_msg())
rospy.sleep(0.5)
def exitForceControl(self):
pub = rospy.Publisher('/robot/limb/right/interaction_control_command',
InteractionControlCommand,
queue_size=1)
interaction_options = InteractionOptions()
interaction_options.set_interaction_control_active(False)
msg = interaction_options.to_msg()
pub.publish(msg)
def set_interaction_options(self, force):
interaction_options = InteractionOptions()
interaction_options.set_interaction_control_active(True)
interaction_options.set_interaction_control_mode([1, 1, 2, 1, 1, 1])
interaction_options.set_in_endpoint_frame(True)
# set the force:
interaction_options.set_force_command([0.0, 0.0, force, 0.0, 0.0, 0.0])
return interaction_options.to_msg()
def _setup_zero_g(self):
########################################
# Setup Intera to Support Zero-G Mode #
########################################
interaction_pub = InteractionPublisher()
interaction_options = InteractionOptions()
interaction_options.set_max_impedance([False])
interaction_options.set_rotations_for_constrained_zeroG(True)
interaction_frame = Pose()
interaction_frame.position.x = 0
interaction_frame.position.y = 0
interaction_frame.position.z = 0
interaction_frame.orientation.x = 0
interaction_frame.orientation.y = 0
interaction_frame.orientation.z = 0
interaction_frame.orientation.w = 1
interaction_options.set_K_impedance([0, 0, 0, 0, 0, 0])
interaction_options.set_K_nullspace([0, 0, 0, 0, 0, 0, 0])
interaction_options.set_interaction_frame(interaction_frame)
rospy.loginfo(interaction_options.to_msg())
rospy.on_shutdown(interaction_pub.send_position_mode_cmd)
self.interaction_publisher = interaction_pub
self.interaction_options = 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():
"""
Initiate constrained zero-G with a desired behavior from the current pose.
The desired behavior can be specified by the optional arguments. By default,
all directions will be set unconstrained when no options are provided.
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 constrained_zeroG.py [arguments: see below]
-p
--> Allow for arbitrary endpoint position with fixed orientation
-o
--> Allow for arbitrary endpoint orientation with fixed position
-ph
--> Allow for arbitrary endpoint position on a horizontal plane (XY plane) with fixed orientation
-yz
--> Allow for arbitrary endpoint position on a vertical YZ plane with fixed orientation
-px
--> Allow for arbitrary endpoint position along x-axis
-px -oy
--> Allow for arbitrary endpoint position along x-axis as well as arbitrary endpoint orientation about y-axis
-ca 1 1 0 1 1 1
--> Allow for arbitrary translational movement along z-axis only
-ca 1 1 1 0 1 1
--> Allow for arbitrary rotational movement about x-axis only
-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)
-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
-kn 0.0
--> Set K_nullspace to [0 0 0 0 0 0 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(
"-p",
"--position_only",
action='store_true',
default=False,
help="Allow for arbitrary endpoint position with fixed orientation")
parser.add_argument(
"-o",
"--orientation_only",
action='store_true',
default=False,
help="Allow for arbitrary endpoint orientation with fixed position")
parser.add_argument(
"-ph",
"--plane_horizontal",
action='store_true',
default=False,
help=
"Allow for arbitrary endpoint position on a horizontal plane (XY plane) with fixed orientation"
)
parser.add_argument(
"-xz",
"--plane_vertical_xz",
action='store_true',
default=False,
help=
"Allow for arbitrary endpoint position on a vertical plane (XZ plane) with fixed orientation"
)
parser.add_argument(
"-yz",
"--plane_vertical_yz",
action='store_true',
default=False,
help=
"Allow for arbitrary endpoint position on a vertical plane (YZ plane) with fixed orientation"
)
parser.add_argument(
"-ns",
"--nullspace_only",
action='store_true',
default=False,
help=
"Allow for arbitrary movement only in the nullspace of the configuration with fixed endpoint pose"
)
parser.add_argument(
"-px",
"--position_x",
action='store_true',
default=False,
help="Allow for arbitrary endpoint position along x-axis")
parser.add_argument(
"-py",
"--position_y",
action='store_true',
default=False,
help="Allow for arbitrary endpoint position along y-axis")
parser.add_argument(
"-pz",
"--position_z",
action='store_true',
default=False,
help="Allow for arbitrary endpoint position along z-axis")
parser.add_argument(
"-ox",
"--orientation_x",
action='store_true',
default=False,
help="Allow for arbitrary endpoint orientation about x-axis")
parser.add_argument(
"-oy",
"--orientation_y",
action='store_true',
default=False,
help="Allow for arbitrary endpoint orientation about y-axis")
parser.add_argument(
"-oz",
"--orientation_z",
action='store_true',
default=False,
help="Allow for arbitrary endpoint orientation about z-axis")
parser.add_argument(
"-ca",
"--constrained_axes",
type=int,
nargs=6,
default=[1, 1, 1, 1, 1, 1],
choices=[0, 1],
help=
"A list of Cartesian axes with maximum stiffness, 0 (zero stiffness) or 1 (maximum stiffness) for each of the 6 directions (3 translational directions followed by 3 rotational directions)"
)
parser.add_argument(
"-ef",
"--in_endpoint_frame",
action='store_true',
default=False,
help=
"Set the desired reference frame to endpoint frame ('right_hand'); otherwise, it is base frame by default"
)
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(
"-kn",
"--K_nullspace",
type=float,
nargs='+',
default=[10.0, 10.0, 7.0, 0.0, 0.0, 0.0, 0.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(
"-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('constrained_zeroG')
# set the interaction control options in the current configuration
interaction_options = InteractionOptions()
# if one of the options is set
unconstrained_axes_default = [0, 0, 0, 0, 0, 0]
unconstrained_axes = unconstrained_axes_default
# create a list of the constrained zero G modes
sum_mode_list = sum(
bool2int([
args.position_only, args.orientation_only, args.plane_horizontal,
args.plane_vertical_xz, args.plane_vertical_yz, args.nullspace_only
]))
# create a list of the free axis options
free_axis_list = bool2int([
args.position_x, args.position_y, args.position_z, args.orientation_x,
args.orientation_y, args.orientation_z
])
sum_free_axis_list = sum(free_axis_list)
zero_stiffness_axes = boolToggle(args.constrained_axes)
sum_zero_stiffness_axes = sum(zero_stiffness_axes)
if (sum_mode_list == 0 and sum_zero_stiffness_axes == 0
and sum_free_axis_list == 0):
rospy.logerr('You need to either set one of the options or specify the ' \
'desired axes of arbitrary movement!')
rospy.logerr(
'The movement of endpoint will be constrained in all directions!')
if (sum_mode_list > 1 and sum_zero_stiffness_axes == 0
and sum_free_axis_list == 0):
rospy.logerr('You can set only one of the options among "pos_only", "ori_only", ' \
'"plane_hor", and "plane_ver"!')
rospy.logerr(
'The movement of endpoint will be constrained in all directions!')
# give an error if the mode options as well as the individual axis options are set
if (sum_mode_list == 1 and sum_zero_stiffness_axes == 0
and sum_free_axis_list > 0):
rospy.logerr(
'The individual axis options cannot be used together with the mode options!'
)
rospy.logerr(
'The movement of endpoint will be constrained in all directions!')
# give an error when the axes are specified by more than one method
if (sum_mode_list == 0 and sum_zero_stiffness_axes > 0
and sum_free_axis_list > 0):
rospy.logerr(
'You can only set the axes either by an array or individual options!'
)
rospy.logerr(
'The movement of endpoint will be constrained in all directions!')
if (sum_mode_list == 1 and sum_zero_stiffness_axes == 0
and sum_free_axis_list >= 0):
if args.position_only:
unconstrained_axes = [1, 1, 1, 0, 0, 0]
if args.orientation_only:
unconstrained_axes = [0, 0, 0, 1, 1, 1]
if args.plane_horizontal:
unconstrained_axes = [1, 1, 0, 0, 0, 0]
if args.plane_vertical_xz:
unconstrained_axes = [1, 0, 1, 0, 0, 0]
if args.plane_vertical_yz:
unconstrained_axes = [0, 1, 1, 0, 0, 0]
if args.nullspace_only:
unconstrained_axes = [0, 0, 0, 0, 0, 0]
# if the axes are specified by an array
if (sum_mode_list == 0 and sum_zero_stiffness_axes > 0
and sum_free_axis_list == 0):
unconstrained_axes = zero_stiffness_axes
# if the axes are specified by individual options
if (sum_mode_list == 0 and sum_zero_stiffness_axes == 0
and sum_free_axis_list > 0):
unconstrained_axes = free_axis_list
# set the stiffness to zero by default
interaction_options.set_K_impedance([0, 0, 0, 0, 0, 0])
# set the axes with maximum stiffness
interaction_options.set_max_impedance(
boolToggle(int2bool(unconstrained_axes)))
interaction_options.set_in_endpoint_frame(args.in_endpoint_frame)
# set nullspace stiffness to zero if nullspace_only option is provided
if args.nullspace_only:
K_nullspace = [0, 0, 0, 0, 0, 0, 0]
else:
K_nullspace = args.K_nullspace
interaction_options.set_K_nullspace(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.')
# always enable the rotations for constrained zero-G
interaction_options.set_rotations_for_constrained_zeroG(True)
# 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)
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 interaction_control(self, position_only = False, orientation_only = False, plane_horizontal = False,
plane_vertical_xz = False, plane_vertical_yz = False, nullspace_only = False,
position_x = False, position_y = False, position_z = False,
orientation_x = False, orientation_y = False, orientation_z = False,
constrained_axes = [1, 1, 1, 1, 1, 1], in_endpoint_frame = False, interaction_frame = [0, 0, 0, 1, 0, 0, 0],
K_nullspace = [10.0, 10.0, 7.0, 0.0, 0.0, 0.0, 0.0], rate = 10):
try:
rospy.Subscriber('zeroG_topic', String, self.zeroG_callback)
self.zeroG = True
rospy.sleep(0.5)
if rate > 0:
rate = rospy.Rate(rate)
elif rate == 0:
rospy.logwarn('Interaction control options will be set only once!')
elif rate < 0:
rospy.logerr('Invalid publish rate!')
# set the interaction control options in the current configuration
interaction_options = InteractionOptions()
# if one of the options is set
unconstrained_axes_default = [0, 0, 0, 0, 0, 0]
unconstrained_axes = unconstrained_axes_default
# create a list of the constrained zero G modes
sum_mode_list = sum(bool2int([position_only, orientation_only, plane_horizontal, plane_vertical_xz, plane_vertical_yz, nullspace_only]))
# create a list of the free axis options
free_axis_list = bool2int([position_x, position_y, position_z, orientation_x, orientation_y, orientation_z])
sum_free_axis_list = sum(free_axis_list)
zero_stiffness_axes = boolToggle(constrained_axes)
sum_zero_stiffness_axes = sum(zero_stiffness_axes)
if (sum_mode_list==0 and sum_zero_stiffness_axes==0 and sum_free_axis_list==0):
rospy.loginfo('Constrained in all directions')
if (sum_mode_list>1 and sum_zero_stiffness_axes==0 and sum_free_axis_list==0):
rospy.logerr('You can set only one of the options among "pos_only", "ori_only", "plane_hor", and "plane_ver"! The movement of endpoint will be constrained in all directions!')
# give an error if the mode options as well as the individual axis options are set
if (sum_mode_list==1 and sum_zero_stiffness_axes==0 and sum_free_axis_list>0):
rospy.logerr('The individual axis options cannot be used together with the mode options! The movement of endpoint will be constrained in all directions!')
# give an error when the axes are specified by more than one method
if (sum_mode_list==0 and sum_zero_stiffness_axes>0 and sum_free_axis_list>0):
rospy.logerr('You can only set the axes either by an array or individual options! The movement of endpoint will be constrained in all directions!')
if (sum_mode_list==1 and sum_zero_stiffness_axes==0 and sum_free_axis_list>=0):
if position_only:
unconstrained_axes = [1, 1, 1, 0, 0, 0]
if orientation_only:
unconstrained_axes = [0, 0, 0, 1, 1, 1]
if plane_horizontal:
unconstrained_axes = [1, 1, 0, 0, 0, 0]
if plane_vertical_xz:
unconstrained_axes = [1, 0, 1, 0, 0, 0]
if plane_vertical_yz:
unconstrained_axes = [0, 1, 1, 0, 0, 0]
if nullspace_only:
unconstrained_axes = [0, 0, 0, 0, 0, 0]
# if the axes are specified by an array
if (sum_mode_list==0 and sum_zero_stiffness_axes>0 and sum_free_axis_list==0):
unconstrained_axes = zero_stiffness_axes
# if the axes are specified by individual options
if (sum_mode_list==0 and sum_zero_stiffness_axes==0 and sum_free_axis_list>0):
unconstrained_axes = free_axis_list
# set the stiffness to zero by default
interaction_options.set_K_impedance([0, 0, 0, 0, 0, 0])
# set the axes with maximum stiffness
interaction_options.set_max_impedance(boolToggle(int2bool(unconstrained_axes)))
interaction_options.set_in_endpoint_frame(in_endpoint_frame)
# set nullspace stiffness to zero if nullspace_only option is provided
if nullspace_only:
K_nullspace = [0, 0, 0, 0, 0, 0, 0]
interaction_options.set_K_nullspace(K_nullspace)
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:
interaction_frame_pose = Pose()
interaction_frame_pose.position.x = interaction_frame[0]
interaction_frame_pose.position.y = interaction_frame[1]
interaction_frame_pose.position.z = interaction_frame[2]
interaction_frame_pose.orientation.w = interaction_frame[3]
interaction_frame_pose.orientation.x = interaction_frame[4]
interaction_frame_pose.orientation.y = interaction_frame[5]
interaction_frame_pose.orientation.z = interaction_frame[6]
interaction_options.set_interaction_frame(interaction_frame_pose)
else:
rospy.logerr('Invalid input to quaternion! The quaternion must be a unit quaternion!')
else:
rospy.logerr('Invalid input to interaction_frame!')
# always enable the rotations for constrained zero-G
interaction_options.set_rotations_for_constrained_zeroG(True)
msg = interaction_options.to_msg()
# print the resultant interaction options once
#rospy.loginfo(msg)
self.icc_pub.publish(msg)
rs = intera_interface.RobotEnable(CHECK_VERSION)
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user. %s',
'Exiting the node...')
if not rs.state().enabled:
self.position_mode()
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)
def main():
"""
Initiate constrained zero-G with a desired behavior from the current pose.
The desired behavior can be specified by the optional arguments. By default,
all directions will be set unconstrained when no options are provided.
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 regular
zero-G, the arm will return to normal position mode. Also, regardless of
the publish rate, the regular zero-G behavior will not be affected by this.
Call using:
$ rosrun intera_examples constrained_zeroG.py [arguments: see below]
-p
--> Allow for arbitrary endpoint position with fixed orientation
-o
--> Allow for arbitrary endpoint orientation with fixed position
-ph
--> Allow for arbitrary endpoint position on a horizontal plane (XY plane) with fixed orientation
-yz
--> Allow for arbitrary endpoint position on a vertical YZ plane with fixed orientation
-px
--> Allow for arbitrary endpoint position along x-axis
-px -oy
--> Allow for arbitrary endpoint position along x-axis as well as arbitrary endpoint orientation about y-axis
-ca 1 1 0 1 1 1
--> Allow for arbitrary translational movement along z-axis only
-ca 1 1 1 0 1 1
--> Allow for arbitrary rotational movement about x-axis only
-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)
-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
-kn 0.0
--> Set K_nullspace to [0 0 0 0 0 0 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(
"-p", "--position_only", action='store_true', default=False,
help="Allow for arbitrary endpoint position with fixed orientation")
parser.add_argument(
"-o", "--orientation_only", action='store_true', default=False,
help="Allow for arbitrary endpoint orientation with fixed position")
parser.add_argument(
"-ph", "--plane_horizontal", action='store_true', default=False,
help="Allow for arbitrary endpoint position on a horizontal plane (XY plane) with fixed orientation")
parser.add_argument(
"-xz", "--plane_vertical_xz", action='store_true', default=False,
help="Allow for arbitrary endpoint position on a vertical plane (XZ plane) with fixed orientation")
parser.add_argument(
"-yz", "--plane_vertical_yz", action='store_true', default=False,
help="Allow for arbitrary endpoint position on a vertical plane (YZ plane) with fixed orientation")
parser.add_argument(
"-ns", "--nullspace_only", action='store_true', default=False,
help="Allow for arbitrary movement only in the nullspace of the configuration with fixed endpoint pose")
parser.add_argument(
"-px", "--position_x", action='store_true', default=False,
help="Allow for arbitrary endpoint position along x-axis")
parser.add_argument(
"-py", "--position_y", action='store_true', default=False,
help="Allow for arbitrary endpoint position along y-axis")
parser.add_argument(
"-pz", "--position_z", action='store_true', default=False,
help="Allow for arbitrary endpoint position along z-axis")
parser.add_argument(
"-ox", "--orientation_x", action='store_true', default=False,
help="Allow for arbitrary endpoint orientation about x-axis")
parser.add_argument(
"-oy", "--orientation_y", action='store_true', default=False,
help="Allow for arbitrary endpoint orientation about y-axis")
parser.add_argument(
"-oz", "--orientation_z", action='store_true', default=False,
help="Allow for arbitrary endpoint orientation about z-axis")
parser.add_argument(
"-ca", "--constrained_axes", type=int,
nargs=6, default=[1, 1, 1, 1, 1, 1], choices = [0, 1],
help="A list of Cartesian axes with maximum stiffness, 0 (zero stiffness) or 1 (maximum stiffness) for each of the 6 directions (3 translational directions followed by 3 rotational directions)")
parser.add_argument(
"-ef", "--in_endpoint_frame", action='store_true', default=False,
help="Set the desired reference frame to endpoint frame ('right_hand'); otherwise, it is base frame by default")
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(
"-kn", "--K_nullspace", type=float,
nargs='+', default=[10.0, 10.0, 7.0, 0.0, 0.0, 0.0, 0.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(
"-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()
# if one of the options is set
unconstrained_axes_default = [0, 0, 0, 0, 0, 0]
unconstrained_axes = unconstrained_axes_default
# create a list of the constrained zero G modes
sum_mode_list = sum(bool2int([args.position_only, args.orientation_only, args.plane_horizontal, args.plane_vertical_xz, args.plane_vertical_yz, args.nullspace_only]))
# create a list of the free axis options
free_axis_list = bool2int([args.position_x, args.position_y, args.position_z, args.orientation_x, args.orientation_y, args.orientation_z])
sum_free_axis_list = sum(free_axis_list)
zero_stiffness_axes = boolToggle(args.constrained_axes)
sum_zero_stiffness_axes = sum(zero_stiffness_axes)
if (sum_mode_list==0 and sum_zero_stiffness_axes==0 and sum_free_axis_list==0):
rospy.logerr('You need to either set one of the options or specify the desired axes of arbitrary movement! The movement of endpoint will be constrained in all directions!')
if (sum_mode_list>1 and sum_zero_stiffness_axes==0 and sum_free_axis_list==0):
rospy.logerr('You can set only one of the options among "pos_only", "ori_only", "plane_hor", and "plane_ver"! The movement of endpoint will be constrained in all directions!')
# give an error if the mode options as well as the individual axis options are set
if (sum_mode_list==1 and sum_zero_stiffness_axes==0 and sum_free_axis_list>0):
rospy.logerr('The individual axis options cannot be used together with the mode options! The movement of endpoint will be constrained in all directions!')
# give an error when the axes are specified by more than one method
if (sum_mode_list==0 and sum_zero_stiffness_axes>0 and sum_free_axis_list>0):
rospy.logerr('You can only set the axes either by an array or individual options! The movement of endpoint will be constrained in all directions!')
if (sum_mode_list==1 and sum_zero_stiffness_axes==0 and sum_free_axis_list>=0):
if args.position_only:
unconstrained_axes = [1, 1, 1, 0, 0, 0]
if args.orientation_only:
unconstrained_axes = [0, 0, 0, 1, 1, 1]
if args.plane_horizontal:
unconstrained_axes = [1, 1, 0, 0, 0, 0]
if args.plane_vertical_xz:
unconstrained_axes = [1, 0, 1, 0, 0, 0]
if args.plane_vertical_yz:
unconstrained_axes = [0, 1, 1, 0, 0, 0]
if args.nullspace_only:
unconstrained_axes = [0, 0, 0, 0, 0, 0]
# if the axes are specified by an array
if (sum_mode_list==0 and sum_zero_stiffness_axes>0 and sum_free_axis_list==0):
unconstrained_axes = zero_stiffness_axes
# if the axes are specified by individual options
if (sum_mode_list==0 and sum_zero_stiffness_axes==0 and sum_free_axis_list>0):
unconstrained_axes = free_axis_list
# set the stiffness to zero by default
interaction_options.set_K_impedance([0, 0, 0, 0, 0, 0])
# set the axes with maximum stiffness
interaction_options.set_max_impedance(boolToggle(int2bool(unconstrained_axes)))
interaction_options.set_in_endpoint_frame(args.in_endpoint_frame)
# set nullspace stiffness to zero if nullspace_only option is provided
if args.nullspace_only:
K_nullspace = [0, 0, 0, 0, 0, 0, 0]
else:
K_nullspace = args.K_nullspace
interaction_options.set_K_nullspace(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!')
# always enable the rotations for constrained zero-G
interaction_options.set_rotations_for_constrained_zeroG(True)
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():
"""
Demonstration Recorder
Record a series of demonstrations.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument('-c',
'--config',
dest='config',
required=True,
help='the file path of the demonstration ')
required.add_argument(
'-d',
'--directory',
dest='directory',
required=True,
help='the directory to save raw demonstration .json files')
parser.add_argument('-r',
'--record_rate',
type=int,
default=50,
metavar='RECORDRATE',
help='rate at which to record (default: 50)')
args = parser.parse_args(rospy.myargv()[1:])
print("Initializing node... ")
rospy.init_node("sdk_joint_recorder")
print("Getting robot state... ")
robot_state = intera_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
robot_state.enable()
interaction_pub = InteractionPublisher()
interaction_options = InteractionOptions()
interaction_options.set_max_impedance([False])
interaction_options.set_rotations_for_constrained_zeroG(True)
interaction_frame = Pose()
interaction_frame.position.x = 0
interaction_frame.position.y = 0
interaction_frame.position.z = 0
interaction_frame.orientation.x = 0
interaction_frame.orientation.y = 0
interaction_frame.orientation.z = 0
interaction_frame.orientation.w = 1
interaction_options.set_K_impedance([0, 0, 0, 0, 0, 0])
interaction_options.set_K_nullspace([0, 0, 0, 0, 0, 0, 0])
interaction_options.set_interaction_frame(interaction_frame)
rospy.loginfo(interaction_options.to_msg())
rospy.on_shutdown(interaction_pub.send_position_mode_cmd)
config_filepath = args.config
configs = import_configuration(config_filepath)
items = ItemFactory(configs).generate_items()
triggers = TriggerFactory(configs).generate_triggers()
constraints = ConstraintFactory(configs).generate_constraints()
# We only have just the one robot...for now.......
environment = Environment(items=items['items'],
robot=items['robots'][0],
constraints=constraints,
triggers=triggers)
exp = DataExporter()
print("Recording. Press Ctrl-C to stop.")
constraint_analyzer = ConstraintAnalyzer(environment)
start_configuration = configs["settings"]["start_configuration"]
recorder = SawyerRecorder(start_configuration, args.record_rate,
interaction_pub, interaction_options)
rospy.on_shutdown(recorder.stop)
demos = recorder.run(environment, constraint_analyzer, auto_zeroG=True)
# Build processors and process demonstrations to generate derivative data e.g. relative position.
rk_processor = RelativeKinematicsProcessor(environment.get_item_ids(),
environment.get_robot_id())
ic_processor = InContactProcessor(environment.get_item_ids(),
environment.get_robot_id(), .06, .5)
soi_processor = SphereOfInfluenceProcessor(environment.get_item_ids(),
environment.get_robot_id())
rp_processor = RelativePositionProcessor(environment.get_item_ids(),
environment.get_robot_id())
wp_processor = WithinPerimeterProcessor(environment.get_item_ids(),
environment.get_robot_id())
pipeline = ProcessorPipeline([
rk_processor, ic_processor, soi_processor, rp_processor, wp_processor
])
pipeline.process(demos)
# Analyze observations for constraints. If using web triggered constraints, we don't evaluate and
# instead the applied constraints are those explicitly set by the user.
for demo in demos:
if configs['settings'][
'constraint_trigger_mechanism'] == 'cuff_trigger':
# Using the cuff trigger will cause a propagation forward.
constraint_analyzer.applied_constraint_evaluator(demo.observations)
elif configs['settings'][
'constraint_trigger_mechanism'] == 'web_trigger':
for observation in demo.observations:
observation.data[
"applied_constraints"] = observation.get_triggered_constraint_data(
)
else:
rospy.logerr("No valid constraint trigger mechanism passed.")
exp = DataExporter()
for idx, demo in enumerate(demos):
raw_data = [obs.data for obs in demo.observations]
print("'/raw_demonstration{}.json': {} observations".format(
idx, len(raw_data)))
exp.export_to_json(
args.directory + "/raw_demonstration{}.json".format(idx), raw_data)
def main():
"""
Demonstration Recorder
Record a series of demonstrations.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument('-c',
'--config',
dest='config',
required=True,
help='the file path of the demonstration ')
parser.add_argument('-r',
'--record_rate',
type=int,
default=45,
metavar='RECORDRATE',
help='rate at which to record (default: 45)')
args = parser.parse_args(rospy.myargv()[1:])
print("Initializing node... ")
rospy.init_node("live_constraint")
print("Getting robot state... ")
robot_state = intera_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
robot_state.enable()
interaction_pub = InteractionPublisher()
interaction_options = InteractionOptions()
interaction_options.set_max_impedance([False])
interaction_options.set_rotations_for_constrained_zeroG(True)
interaction_frame = Pose()
interaction_frame.position.x = 0
interaction_frame.position.y = 0
interaction_frame.position.z = 0
interaction_frame.orientation.x = 0
interaction_frame.orientation.y = 0
interaction_frame.orientation.z = 0
interaction_frame.orientation.w = 1
interaction_options.set_K_impedance([0, 0, 0, 0, 0, 0])
interaction_options.set_K_nullspace([5, 5, 5, 5, 5, 5, 5])
interaction_options.set_interaction_frame(interaction_frame)
rospy.loginfo(interaction_options.to_msg())
rospy.on_shutdown(interaction_pub.send_position_mode_cmd)
interaction_pub.external_rate_send_command(interaction_options)
config_filepath = args.config
configs = import_configuration(config_filepath)
items = ItemFactory(configs).generate_items()
triggers = TriggerFactory(configs).generate_triggers()
constraints = ConstraintFactory(configs).generate_constraints()
constraint_ids = [constraint.id for constraint in constraints]
print("Constraint IDs: {}".format(constraint_ids))
# We only have just the one robot...for now.......
environment = Environment(items=items['items'],
robot=items['robots'][0],
constraints=constraints,
triggers=triggers)
constraint_analyzer = ConstraintAnalyzer(environment)
user_input = ""
while environment.robot._navigator.get_button_state(
"right_button_back") != 2 or user_input == "q":
stdin, stdout, stderr = select.select([sys.stdin], [], [], .0001)
for s in stdin:
if s == sys.stdin:
user_input = sys.stdin.readline().strip()
data = {
"robot": environment.get_robot_state(),
"items": environment.get_item_state(),
"triggered_constraints": environment.check_constraint_triggers()
}
observation = Observation(data)
print "Position: " + str(data["robot"]["position"])
print "Orientation: " + str(data["robot"]["orientation"])
print "Config" + str(data["robot"]["joint_angle"])
print(constraint_analyzer.evaluate(constraints, observation))
print(data["triggered_constraints"])
valid_constraints = constraint_analyzer.evaluate(
environment.constraints, observation)[1]
pub = rospy.Publisher('cairo_lfd/valid_constraints',
Int8MultiArray,
queue_size=10)
msg = Int8MultiArray(data=valid_constraints)
pub.publish(msg)
rospy.sleep(1)
if rospy.is_shutdown():
return 1