def _get_fk_pykdl(self, frame_id, state=None):
if state is None:
state = self.get_current_state()
fk = self._kinematics_pykdl.forward_position_kinematics(
dict(zip(state.joint_state.name, state.joint_state.position)))
ps = list_to_pose([fk[:3], fk[-4:]], self._world)
return self._tf_listener.transformPose(frame_id, ps)
def compute_sub_ik(self, group, desired_dict, result, tol=0.001):
# get the desired pose in the correct base frame
index = self.links.index(group)
base = self.bases[index]
tr = desired_dict[group]
if base != self.prefix + '/base':
if base in desired_dict.keys():
tr_base = desired_dict[base]
inv_base = transformations.inverse_transform(tr_base)
desired_pose = transformations.multiply_transform(inv_base, tr)
else:
return 1
else:
desired_pose = tr
# transform it back to PoseStamped
desired_pose = transformations.list_to_pose(desired_pose)
try:
# call the srv
res = self.div_ik_srv[group](desired_poses=[desired_pose], tolerance=tol)
joints = res.joint_state.position
except:
return 1
with self.lock:
result[group] = joints
def get_ik(self, eef_poses, seeds=(), source=None, params=None):
"""
Return IK solutions of this arm's end effector according to the method declared in the constructor
:param eef_poses: a PoseStamped or a list [[x, y, z], [x, y, z, w]] in world frame or a list of PoseStamped
:param seeds: a single seed or a list of seeds of type RobotState for each input pose
:param source: 'robot', 'trac', 'kdl'... the IK source for this call (warning: the source might not be instanciated)
:param params: dictionary containing optional non-generic IK parameters
:return: a list of RobotState for each input pose in input or a single RobotState
TODO: accept also a Point (baxter_pykdl's IK accepts orientation=None)
Child methods wait for a *list* of pose(s) and a *list* of seed(s) for each pose
"""
if not isinstance(eef_poses, list) or isinstance(eef_poses[0], list) and not isinstance(eef_poses[0][0], list):
eef_poses = [eef_poses]
if not seeds:
seeds=[]
elif not isinstance(seeds, list):
seeds = [seeds]*len(eef_poses)
input = []
for eef_pose in eef_poses:
if isinstance(eef_pose, list):
input.append(list_to_pose(eef_pose, self._world))
elif isinstance(eef_pose, PoseStamped):
input.append(eef_pose)
else:
raise TypeError("ArmCommander.get_ik() accepts only a list of Postamped or [[x, y, z], [x, y, z, w]], got {}".format(str(type(eef_pose))))
output = self._kinematics_services['ik'][self._selected_ik if source is None else source]['func'](input, seeds, params)
return output if len(eef_poses)>1 else output[0]
def dicttopath(dic):
path = Path()
path.header.frame_id = dic["frame_id"]
for point in dic["points"]:
path.poses.append(list_to_pose(point["pose"], frame_id=dic["frame_id"]))
path.poses[-1].header.stamp = Time(point["time"])
return path
def dicttopath(dic):
path = Path()
path.header.frame_id = dic["frame_id"]
for point in dic["points"]:
path.poses.append(list_to_pose(point["pose"],
frame_id=dic["frame_id"]))
path.poses[-1].header.stamp = Time(point["time"])
return path
def _get_fk_robot(self, frame_id=None, state=None):
# Keep this half-working FK, still used by generate_cartesian_path (trajectories.py)
if state is not None:
raise NotImplementedError(
"_get_fk_robot has no FK service provided by the robot except for its current endpoint pose"
)
ps = list_to_pose(self.endpoint_pose(), self._world)
return self._tf_listener.transformPose(frame_id, ps)
def compute_sub_ik(self, group, desired_dict, result, seed, tol=0.1):
# get the desired pose in the correct base frame
base = self.links[group]
tr = desired_dict[group]
if desired_dict:
base_found = False
if base != self.prefix + '/base':
if base in desired_dict.keys():
base_found = True
tr_base = desired_dict[base]
else:
# look for the base in already calculated results
timeout = 0.25
start = time()
while not base_found and not rospy.is_shutdown() and (
time() - start < timeout):
with self.lock:
for key, value in result.iteritems():
if base in self.model.get_links_chain(
self.links[key], key):
base_found = True
js = JointState()
js.name = value['joint_names']
js.position = value['joint_values']
# call the forward kinematic
fk = self.model.forward_kinematic(
js, links=base)
tr_base = fk[base]
if base_found:
inv_base = transformations.inverse_transform(tr_base)
desired_pose = transformations.multiply_transform(
inv_base, tr)
else:
return 1
else:
desired_pose = tr
else:
return 1
# transform it back to PoseStamped
desired_pose = transformations.list_to_pose(desired_pose)
desired_pose.header.frame_id = base
# try:
# call the srv
res = self.div_ik_srv[group](desired_poses=[desired_pose],
tolerance=tol,
seed=seed)
joint_state = res.joint_state
# except Exception, e:
# print e
# return 1
with self.lock:
result[group] = {
'joint_names': joint_state.name,
'joint_values': joint_state.position
}
def _object_grasp_pose_to_world(self, poselist, object):
"""
Returns the transformation from world to the point defined by "poselist attached to object frame" aka
a constraint or a grasp pose.
:param poselist: A poselist [[x, y, z], [x, y, z, w]]
:param object: The frame where the pose should be attached
:return: the transformation from world to the point defined by "poselist attached to object frame" (PoseStamped)
"""
obj_pose = transformations.list_to_pose(poselist, object)
world_pose = self.tfl.transformPose(self.world, obj_pose)
return world_pose
def rollout(self, goal):
assert isinstance(goal, PoseStamped)
self._dmp.goal = [val for sublist in pose_to_list(goal) for val in sublist]
y_track, dy_track, ddy_track = self._dmp.rollout()
path = Path()
for y in y_track:
path.poses.append(list_to_pose([[y[0], y[1], y[2]], [y[3], y[4], y[5], y[6]]],
frame_id=self.init_path.header.frame_id))
path.header.stamp = rospy.Time.now()
path.header.frame_id = self.init_path.header.frame_id
return path
def rollout(self, goal):
assert isinstance(goal, PoseStamped)
self._dmp.goal = [
val for sublist in pose_to_list(goal) for val in sublist
]
y_track, dy_track, ddy_track = self._dmp.rollout()
path = Path()
for y in y_track:
path.poses.append(
list_to_pose([[y[0], y[1], y[2]], [y[3], y[4], y[5], y[6]]],
frame_id=self.init_path.header.frame_id))
path.header.stamp = rospy.Time.now()
path.header.frame_id = self.init_path.header.frame_id
return path
def get_ik(self, eef_poses, seeds=(), source=None, params=None):
"""
Return IK solutions of this arm's end effector according to the method declared in the constructor
:param eef_poses: a PoseStamped or a list [[x, y, z], [x, y, z, w]] in world frame or a list of PoseStamped
:param seeds: a single seed or a list of seeds of type RobotState for each input pose
:param source: 'robot', 'trac', 'kdl'... the IK source for this call (warning: the source might not be instanciated)
:param params: dictionary containing optional non-generic IK parameters
:return: a list of RobotState for each input pose in input or a single RobotState
TODO: accept also a Point (baxter_pykdl's IK accepts orientation=None)
Child methods wait for a *list* of pose(s) and a *list* of seed(s) for each pose
"""
if not isinstance(eef_poses, list) or isinstance(
eef_poses[0], list) and not isinstance(eef_poses[0][0], list):
eef_poses = [eef_poses]
if not seeds:
seeds = []
elif not isinstance(seeds, list):
seeds = [seeds] * len(eef_poses)
input = []
for eef_pose in eef_poses:
if isinstance(eef_pose, list):
input.append(list_to_pose(eef_pose, self._world))
elif isinstance(eef_pose, PoseStamped):
input.append(eef_pose)
else:
raise TypeError(
"ArmCommander.get_ik() accepts only a list of Postamped or [[x, y, z], [x, y, z, w]], got {}"
.format(str(type(eef_pose))))
output = self._kinematics_services['ik'][
self._selected_ik if source is None else source]['func'](input,
seeds,
params)
return output if len(eef_poses) > 1 else output[0]
def __init__(self, side):
# General attributes
self.rospack = rospkg.RosPack()
self.side = side
self.arm_name = side + '_arm'
self.gripper_name = side + '_gripper'
# Transform/Geometric attributes
self.tfl = tf.TransformListener(True, rospy.Duration(
5 *
60)) # TF Interpolation ON and duration of its cache = 5 minutes
self.world = "base"
self.scene = rospy.get_param("/thr/scene")
with open(
self.rospack.get_path("thr_scenes") + "/config/" + self.scene +
"/poses.json") as f:
self.poses = json.load(f)
with open(
self.rospack.get_path("thr_action_server") +
"/config/action_params.json") as f:
self.action_params = json.load(f)
with open(
self.rospack.get_path("thr_action_server") +
"/config/seeds.json") as f:
self.seeds = json.load(f)
with open(
self.rospack.get_path("thr_action_server") +
"/config/abilities.json") as f:
self.abilities = json.load(f)
# Motion/Grasping attributes
self.commander = ArmCommander(
side,
default_kv_max=self.action_params['limits']['kv'],
default_ka_max=self.action_params['limits']['ka'],
ik='robot',
fk='kdl')
# Home poses are taken when the server starts:
self.starting_state = self.commander.get_current_state()
self.tfl.waitForTransform(self.world, self.gripper_name, rospy.Time(0),
rospy.Duration(10))
self.starting_pose = transformations.list_to_pose(
self.tfl.lookupTransform(self.world, self.gripper_name,
rospy.Time(0)))
# Checking that the workstation has an acceptable time offset with the robot
robot_current_time = self.tfl.getLatestCommonTime(
'base', self.gripper_name).to_sec()
local_current_time = time()
diff = int(abs(local_current_time - robot_current_time) * 1000)
if diff > 250:
raise ValueError(
"Your workstation's time has an offset of {} ms with the robot,"
"please synchronize them to prevent using outdated transforms (sudo ntpdate)"
.format(diff))
# Action server attributes
rospy.loginfo("Starting server " + side)
self.server = actionlib.SimpleActionServer(
'/thr/robot_run_action/' + side, RunRobotActionAction,
self.execute, False)
self.result = RunRobotActionActionResult()
# Actual actions
self.actions = {
'give':
Give(self.commander, self.tfl, self.action_params, self.poses,
self.seeds, self.server.is_preempt_requested),
'go_home_' + self.side:
GoHome(self.commander, self.tfl, self.action_params, self.poses,
self.seeds, self.server.is_preempt_requested),
'hold':
Hold(self.commander, self.tfl, self.action_params, self.poses,
self.seeds, self.server.is_preempt_requested),
'pick':
Pick(self.commander, self.tfl, self.action_params, self.poses,
self.seeds, self.server.is_preempt_requested),
'grasp':
Grasp(self.commander, self.tfl, self.action_params, self.poses,
self.seeds, self.server.is_preempt_requested),
'bring_' + self.side:
Bring(self.commander, self.tfl, self.action_params, self.poses,
self.seeds, self.server.is_preempt_requested),
'place_' + self.side:
Place(self.commander, self.tfl, self.action_params, self.poses,
self.seeds, self.server.is_preempt_requested),
}
# On starting blocks...
self.server.start()
rospy.loginfo(side + ' server ready')
res = compute_ik(poses, fixed_joint_state, tolerance,
group_names, seed)
return res.joint_state
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# in case of failure return T pose
return self.get_initial_state()
if seed is None:
seed = self.get_current_state()
if group_names is not list:
group_names = [group_names]
# convert the desired poses to PoseStamped
poses = []
for key, value in desired_poses.iteritems():
pose = transformations.list_to_pose(value)
pose.header.frame_id = key
poses.append(pose)
# convert the fixed joints to joint state
fixed_joint_state = JointState()
for key, value in fixed_joints.iteritems():
fixed_joint_state.name += [key]
fixed_joint_state.position += [value]
return compute_ik_client()
def get_joint_values(self, group_name, joint_names=None):
if joint_names is None or not joint_names:
return self.groups[group_name].get_current_joint_values()
else:
joint_values = self.groups[group_name].get_current_joint_values()
res = []
try:
compute_ik = rospy.ServiceProxy('compute_human_ik', GetHumanIK)
res = compute_ik(poses, fixed_joint_state, tolerance, group_names, seed)
return res.joint_state
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# in case of failure return T pose
return self.get_initial_state()
if seed is None:
seed = self.get_current_state()
if group_names is not list:
group_names = [group_names]
# convert the desired poses to PoseStamped
poses = []
for key, value in desired_poses.iteritems():
pose = transformations.list_to_pose(value)
pose.header.frame_id = key
poses.append(pose)
# convert the fixed joints to joint state
fixed_joint_state = JointState()
for key, value in fixed_joints.iteritems():
fixed_joint_state.name += [key]
fixed_joint_state.position += [value]
return compute_ik_client()
def get_joint_values(self, group_name, joint_names=None):
if joint_names is None or not joint_names:
return self.groups[group_name].get_current_joint_values()
else:
joint_values = self.groups[group_name].get_current_joint_values()
res = []
# Repeat the last point (stable state) n times to avoid brutal cuts due to asymptotic approach
for n in range(nss):
y_des.append(y_des[-1])
return np.array(y_des).transpose()
def rollout(self, goal):
assert isinstance(goal, PoseStamped)
self._dmp.goal = [val for sublist in pose_to_list(goal) for val in sublist]
y_track, dy_track, ddy_track = self._dmp.rollout()
path = Path()
for y in y_track:
path.poses.append(list_to_pose([[y[0], y[1], y[2]], [y[3], y[4], y[5], y[6]]],
frame_id=self.init_path.header.frame_id))
path.header.stamp = rospy.Time.now()
path.header.frame_id = self.init_path.header.frame_id
return path
if __name__=='__main__':
rospy.init_node("test_dmp_traj_discrete")
path = Path()
for i in range(100):
quat = quaternion_about_axis(i*0.00628, (1, 0, 0))
pose = list_to_pose([[i/100., i/100., i/100.], quat])
pose.header.stamp = i/10.
path.poses.append(pose)
goal = list_to_pose([[-10, -10, -10], [0.707, 0, 0, 0.707]])
rollout = DiscreteTaskSpaceTrajectory(path).rollout(goal)
print rollout
def _get_fk_robot(self, frame_id = None, state=None):
# Keep this half-working FK, still used by generate_cartesian_path (trajectories.py)
if state is not None:
raise NotImplementedError("_get_fk_robot has no FK service provided by the robot except for its current endpoint pose")
ps = list_to_pose(self.endpoint_pose(), self._world)
return self._tf_listener.transformPose(frame_id, ps)
return np.array(y_des).transpose()
def rollout(self, goal):
assert isinstance(goal, PoseStamped)
self._dmp.goal = [
val for sublist in pose_to_list(goal) for val in sublist
]
y_track, dy_track, ddy_track = self._dmp.rollout()
path = Path()
for y in y_track:
path.poses.append(
list_to_pose([[y[0], y[1], y[2]], [y[3], y[4], y[5], y[6]]],
frame_id=self.init_path.header.frame_id))
path.header.stamp = rospy.Time.now()
path.header.frame_id = self.init_path.header.frame_id
return path
if __name__ == '__main__':
rospy.init_node("test_dmp_traj_discrete")
path = Path()
for i in range(100):
quat = quaternion_about_axis(i * 0.00628, (1, 0, 0))
pose = list_to_pose([[i / 100., i / 100., i / 100.], quat])
pose.header.stamp = i / 10.
path.poses.append(pose)
goal = list_to_pose([[-10, -10, -10], [0.707, 0, 0, 0.707]])
rollout = DiscreteTaskSpaceTrajectory(path).rollout(goal)
print rollout