def goto_mp(self,
movement_primitive: MovementPrimitive,
frequency=50,
duration=10.,
group_name="arm_gripper"):
curr_controller = self._ctrl_manager.set_motion_controller(
self._ctrl_manager.joint_trajectory_controller)
traj_client = JointTrajectoryActionClient(
joint_names=self.groups[group_name].refs)
traj_client.clear()
pos_traj = movement_primitive.get_full_trajectory(frequency=frequency,
duration=duration)
vel_traj = movement_primitive.get_full_trajectory_derivative(
frequency=frequency, duration=duration)
timing = []
positions = []
velocities = []
d_tot = 0
for goal, d in pos_traj:
d_tot += float(d)
timing.append(d_tot)
positions.append([goal[k] for k in self.groups[group_name].refs])
for goal, d in vel_traj:
velocities.append(
[goal[k] / duration for k in self.groups[group_name].refs])
for t, position, velocity in zip(timing, positions, velocities):
traj_client.add_point(positions=position,
time=t,
velocities=velocity)
print("COMMAND SENT")
traj_client.start() # send the trajectory action request
# traj_client.wait(timeout = timeout)
franka_dataflow.wait_for(test=lambda: traj_client.result(),
timeout=60,
timeout_msg="Something did not work",
rate=100,
raise_on_error=False)
res = traj_client.result()
if res is not None and res.error_code:
rospy.loginfo("Trajectory Server Message: {}".format(res))
rospy.sleep(0.5)
self._ctrl_manager.set_motion_controller(curr_controller)
rospy.loginfo("{}: Trajectory controlling complete".format(
self.__class__.__name__))
rospy.sleep(0.5)
def move_from_touch(self, positions, timeout=10.0, threshold=0.00085):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter to smooth the movement.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
@type threshold: float
@param threshold: position threshold in radians across each joint when
move is considered successful [0.008726646]
@param test: optional function returning True if motion must be aborted
"""
if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller:
self.switchToController(
self._ctrl_manager.joint_trajectory_controller)
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names())
traj_client.clear()
dur = []
for j in range(len(self._joint_names)):
dur.append(
max(
abs(positions[self._joint_names[j]] -
self._joint_angle[self._joint_names[j]]) /
self._joint_limits.velocity[j], min_traj_dur))
traj_client.add_point(
positions=[positions[n] for n in self._joint_names],
time=max(dur) / self._speed_ratio)
diffs = [
self.genf(j, a) for j, a in positions.items()
if j in self._joint_angle
]
fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format(
self.name.capitalize())
traj_client.start() # send the trajectory action request
franka_dataflow.wait_for(test=lambda:
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg="Unable to complete plan!",
rate=100,
raise_on_error=False)
rospy.sleep(0.5)
rospy.loginfo("ArmInterface: Trajectory controlling complete")
def move_to_joint_positions(self,
positions,
timeout=10.0,
threshold=0.00085,
test=None,
use_moveit=True):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter using JointTrajectoryService
to smooth the movement or optionally uses MoveIt! to plan and
execute a trajectory.
:type positions: dict({str:float})
:param positions: joint_name:angle command
:type timeout: float
:param timeout: seconds to wait for move to finish [15]
:type threshold: float
:param threshold: position threshold in radians across each joint when
move is considered successful [0.00085]
:param test: optional function returning True if motion must be aborted
:type use_moveit: bool
:param use_moveit: if set to True, and movegroup interface is available,
move to the joint positions using moveit planner.
"""
curr_controller = self._ctrl_manager.set_motion_controller(
self._ctrl_manager.joint_trajectory_controller)
if use_moveit and self._movegroup_interface:
self._movegroup_interface.go_to_joint_positions(
[positions[n] for n in self._joint_names], tolerance=threshold)
else:
if use_moveit:
rospy.logwarn(
"{}: MoveGroupInterface was not found! Using JointTrajectoryActionClient instead."
.format(self.__class__.__name__))
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names())
traj_client.clear()
dur = []
for j in range(len(self._joint_names)):
dur.append(
max(
abs(positions[self._joint_names[j]] -
self._joint_angle[self._joint_names[j]]) /
self._joint_limits.velocity[j], min_traj_dur))
traj_client.add_point(
positions=[self._joint_angle[n] for n in self._joint_names],
time=0.0001)
traj_client.add_point(
positions=[positions[n] for n in self._joint_names],
time=max(dur) / self._speed_ratio)
def genf(joint, angle):
def joint_diff():
return abs(angle - self._joint_angle[joint])
return joint_diff
diffs = [
genf(j, a) for j, a in positions.items()
if j in self._joint_angle
]
fail_msg = "{}: {} limb failed to reach commanded joint positions.".format(
self.__class__.__name__, self.name.capitalize())
def test_collision():
if self.has_collided():
rospy.logerr(' '.join(["Collision detected.", fail_msg]))
return True
return False
traj_client.start() # send the trajectory action request
# traj_client.wait(timeout = timeout)
franka_dataflow.wait_for(
test=lambda: test_collision() or traj_client.result(
) is not None or (callable(test) and test() == True) or
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg=fail_msg,
rate=100,
raise_on_error=False)
res = traj_client.result()
if res is not None and res.error_code:
rospy.loginfo("Trajectory Server Message: {}".format(res))
rospy.sleep(0.5)
self._ctrl_manager.set_motion_controller(curr_controller)
rospy.loginfo("{}: Trajectory controlling complete".format(
self.__class__.__name__))
def go_to(self,
trajectory: NamedTrajectoryBase,
group_name="arm_gripper",
frequency=50):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter using JointTrajectoryService
to smooth the movement or optionally uses MoveIt! to plan and
execute a trajectory.
:type positions: dict({str:float})
:param positions: joint_name:angle command
:type timeout: float
:param timeout: seconds to wait for move to finish [15]
:type threshold: float
:param threshold: position threshold in radians across each joint when
move is considered successful [0.00085]
:param test: optional function returning True if motion must be aborted
:type use_moveit: bool
:param use_moveit: if set to True, and movegroup interface is available,
move to the joint positions using moveit planner.
"""
curr_controller = self._ctrl_manager.set_motion_controller(
self._ctrl_manager.joint_trajectory_controller)
# traj_client = JointTrajectoryActionClient(
# joint_names=self.joint_names())
traj_client = JointTrajectoryActionClient(
joint_names=self.groups[group_name].refs)
traj_client.clear()
if len(trajectory.duration) > 1:
self._get_cubic_spline(trajectory, group_name)
for i, (position, velocity, d) \
in enumerate(zip(*self._get_cubic_spline(trajectory, group_name, frequency=frequency))):
traj_client.add_point(positions=position.tolist(),
time=float(d),
velocities=velocity.tolist())
else:
for goal, d in trajectory:
position = [
float(goal[k]) for k in self.groups[group_name].refs
]
velocity = [0.] * len(position)
print("position", position)
print("velocity", velocity)
print("d", float(d))
traj_client.add_point(positions=position,
time=float(d),
velocities=velocity)
traj_client.start() # send the trajectory action request
# traj_client.wait(timeout = timeout)
franka_dataflow.wait_for(test=lambda: traj_client.result(),
timeout=260,
timeout_msg="Something did not work",
rate=100,
raise_on_error=False)
res = traj_client.result()
if res is not None and res.error_code:
rospy.loginfo("Trajectory Server Message: {}".format(res))
rospy.sleep(0.5)
self._ctrl_manager.set_motion_controller(curr_controller)
rospy.loginfo("{}: Trajectory controlling complete".format(
self.__class__.__name__))
rospy.sleep(0.5)
def move_to_joint_positions(self,
positions,
timeout=10.0,
threshold=0.00085,
test=None):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter to smooth the movement.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
@type threshold: float
@param threshold: position threshold in radians across each joint when
move is considered successful [0.008726646]
@param test: optional function returning True if motion must be aborted
"""
if self._params._in_sim:
rospy.warn(
"ArmInterface: move_to_joint_positions not implemented for simulation. Use set_joint_positions instead."
)
return
switch_ctrl = True if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller else False
if switch_ctrl:
active_controllers = self._ctrl_manager.list_active_controllers(
only_motion_controllers=True)
for ctrlr in active_controllers:
self._ctrl_manager.stop_controller(ctrlr.name)
rospy.loginfo(
"ArmInterface: Stopping %s for trajectory controlling" %
ctrlr.name)
rospy.sleep(0.5)
if not self._ctrl_manager.is_loaded(
self._ctrl_manager.joint_trajectory_controller):
self._ctrl_manager.load_controller(
self._ctrl_manager.joint_trajectory_controller)
# if self._ctrl_manager.joint_trajectory_controller not in self._ctrl_manager.list_active_controller_names():
self._ctrl_manager.start_controller(
self._ctrl_manager.joint_trajectory_controller)
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names(), ns=self._ns)
traj_client.clear()
dur = []
for j in range(len(self._joint_names)):
dur.append(
max(
abs(positions[self._joint_names[j]] -
self._joint_angle[self._joint_names[j]]) /
self._joint_limits.velocity[j], min_traj_dur))
traj_client.add_point(
positions=[positions[n] for n in self._joint_names],
time=max(dur) / self._speed_ratio)
def genf(joint, angle):
def joint_diff():
return abs(angle - self._joint_angle[joint])
return joint_diff
diffs = [
genf(j, a) for j, a in positions.items() if j in self._joint_angle
]
fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format(
self.name.capitalize())
def test_collision():
if self.has_collided():
rospy.logerr(' '.join(["Collision detected.", fail_msg]))
return True
return False
traj_client.start() # send the trajectory action request
# traj_client.wait(timeout = timeout)
franka_dataflow.wait_for(
test=lambda: test_collision() or \
(callable(test) and test() == True) or \
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg=fail_msg,
rate=100,
raise_on_error=False
)
rospy.sleep(0.5)
if switch_ctrl:
self._ctrl_manager.stop_controller(
self._ctrl_manager.joint_trajectory_controller)
for ctrlr in active_controllers:
self._ctrl_manager.start_controller(ctrlr.name)
rospy.loginfo("ArmInterface: Restaring %s" % ctrlr.name)
rospy.sleep(0.5)
rospy.loginfo("ArmInterface: Trajectory controlling complete")
def move_to_touch(self, positions, timeout=10.0, threshold=0.00085):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter to smooth the movement.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
@type threshold: float
@param threshold: position threshold in radians across each joint when
move is considered successful [0.008726646]
@param test: optional function returning True if motion must be aborted
"""
if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller:
self.switchToController(
self._ctrl_manager.joint_trajectory_controller)
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names())
traj_client.clear()
speed_ratio = 0.05 # Move slower when approaching contact
dur = []
for j in range(len(self._joint_names)):
dur.append(
max(
abs(positions[self._joint_names[j]] -
self._joint_angle[self._joint_names[j]]) /
self._joint_limits.velocity[j], min_traj_dur))
traj_client.add_point(
positions=[positions[n] for n in self._joint_names],
time=max(dur) / speed_ratio,
velocities=[0.002 for n in self._joint_names])
diffs = [
self.genf(j, a) for j, a in positions.items()
if j in self._joint_angle
]
fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format(
self.name.capitalize())
traj_client.start() # send the trajectory action request
franka_dataflow.wait_for(
test=lambda: self.has_collided() or \
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg="Move to touch complete.",
rate=100,
raise_on_error=False
)
rospy.sleep(0.5)
if not self.has_collided():
rospy.logerr('Move To Touch did not end in making contact')
else:
rospy.loginfo('Collision detected!')
# The collision, though desirable, triggers a cartesian reflex error. We need to reset that error
if self._robot_mode == 4:
self.resetErrors()
rospy.loginfo("ArmInterface: Trajectory controlling complete")
def execute_position_path(self,
position_path,
timeout=15.0,
threshold=0.00085,
test=None):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter to smooth the movement.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
@type threshold: float
@param threshold: position threshold in radians across each joint when
move is considered successful [0.008726646]
@param test: optional function returning True if motion must be aborted
"""
current_q = self.joint_angles()
diff_from_start = sum(
[abs(a - current_q[j]) for j, a in position_path[0].items()])
if diff_from_start > 0.1:
raise IOError("Robot not at start of trajectory")
if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller:
self.switchToController(
self._ctrl_manager.joint_trajectory_controller)
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names())
traj_client.clear()
time_so_far = 0
# Start at the second waypoint because robot is already at first waypoint
for i in xrange(1, len(position_path)):
q = position_path[i]
dur = []
for j in range(len(self._joint_names)):
dur.append(
max(
abs(q[self._joint_names[j]] -
self._joint_angle[self._joint_names[j]]) /
self._joint_limits.velocity[j], min_traj_dur))
time_so_far += max(dur) / self._speed_ratio
traj_client.add_point(
positions=[q[n] for n in self._joint_names],
time=time_so_far,
velocities=[0.005 for n in self._joint_names])
diffs = [
self.genf(j, a) for j, a in (position_path[-1]).items()
if j in self._joint_angle
] # Measures diff to last waypoint
fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format(
self.name.capitalize())
def test_collision():
if self.has_collided():
rospy.logerr(' '.join(["Collision detected.", fail_msg]))
return True
return False
traj_client.start() # send the trajectory action request
franka_dataflow.wait_for(
test=lambda: test_collision() or \
(callable(test) and test() == True) or \
(all(diff() < threshold for diff in diffs)),
#timeout=timeout,
timeout=max(time_so_far, timeout), #XXX
timeout_msg=fail_msg,
rate=100,
raise_on_error=False
)
rospy.sleep(0.5)
rospy.loginfo("ArmInterface: Trajectory controlling complete")
def move_to_joint_positions(self,
positions,
timeout=10.0,
threshold=0.00085,
test=None):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter using JointTrajectoryService
to smooth the movement or optionally uses MoveIt! to plan and
execute a trajectory.
:type positions: dict({str:float})
:param positions: joint_name:angle command
:type timeout: float
:param timeout: seconds to wait for move to finish [15]
:type threshold: float
:param threshold: position threshold in radians across each joint when
move is considered successful [0.00085]
:param test: optional function returning True if motion must be aborted
"""
if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller:
self.switchToController(
self._ctrl_manager.joint_trajectory_controller)
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names())
traj_client.clear()
dur = []
for j in range(len(self._joint_names)):
dur.append(
max(
abs(positions[self._joint_names[j]] -
self._joint_angle[self._joint_names[j]]) /
self._joint_limits.velocity[j], min_traj_dur))
traj_client.add_point(
positions=[positions[n] for n in self._joint_names],
time=max(dur) / self._speed_ratio)
diffs = [
self.genf(j, a) for j, a in positions.items()
if j in self._joint_angle
]
traj_client.start() # send the trajectory action request
fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format(
self.name.capitalize())
def test_collision():
if self.has_collided():
rospy.logerr(' '.join(["Collision detected.", fail_msg]))
return True
return False
franka_dataflow.wait_for(
test=lambda: test_collision() or \
(callable(test) and test() == True) or \
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg=fail_msg,
rate=100,
raise_on_error=False
)
res = traj_client.result()
if res is not None and res.error_code:
rospy.loginfo("Trajectory Server Message: {}".format(res))
rospy.sleep(0.5)
rospy.loginfo("ArmInterface: Trajectory controlling complete")