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