def process_body_marker_feedback(self, feedback):
"""Processes the feedback sent by markers corresponding to dynamic objects."""
if feedback.event_type == InteractiveMarkerFeedbackMsg.MOUSE_DOWN and feedback.marker_name != self._selected_object:
self.select_marker(feedback.marker_name)
if feedback.event_type == InteractiveMarkerFeedbackMsg.POSE_UPDATE:
if feedback.marker_name != self._selected_object:
self.select_marker(feedback.marker_name)
if self.is_running():
self.pause()
self.place_mode = True
if feedback.event_type == InteractiveMarkerFeedbackMsg.MOUSE_UP:
body = self.sim.bodies[self._selected_object]
obj_pose = body.pose()
rel_frame = pose_msg_to_frame(feedback.pose)
new_pos, new_rot = pb.multiplyTransforms(
list(obj_pose.position), list(obj_pose.quaternion),
list(rel_frame.position), list(rel_frame.quaternion))
body.set_pose(Frame(new_pos, new_rot), True)
intMarker, cb = self.markers[self._selected_object]
intMarker.pose = zero_pose
self.marker_server.insert(intMarker, cb)
self.marker_server.applyChanges()
if self.place_mode:
self.place_mode = False
self.run()
def srv_set_pose(self, req):
print('srv_set_pose() called')
"""Sets the pose of an object."""
with self.lock:
res = SetObjectPoseResponse()
res.success = False
body = self.sim.get_body(req.object_id)
if body is None:
res.error_msg = 'Object {} does not exist.'.format(
req.object_id)
return res
try:
pos = [
req.pose.position.x, req.pose.position.y,
req.pose.position.z
]
quat = [
req.pose.orientation.x, req.pose.orientation.y,
req.pose.orientation.z, req.pose.orientation.w
]
body.set_pose(Frame(pos, quat), req.override_initial)
res.success = True
return res
except Exception as e:
traceback.print_exc()
res.error_msg = str(e)
return res
def create_constraint_global(self,
parentBody,
childBody,
jointType='fixed',
jointPosition=[0, 0, 0],
jointAxis=[1, 0, 0],
parentLink=None,
childLink=None,
name_override=None):
parent_pose = parentBody.get_link_state(
parentLink) if parentLink is not None else parentBody.pose()
if childBody is not None:
child_pose = childBody.get_link_state(
childLink) if childLink is not None else childBody.pose()
else:
child_pose = Frame((0, 0, 0), (0, 0, 0, 1))
inv_pp_pos, inv_pp_rot = pb.invertTransform(parent_pose.position,
parent_pose.quaternion)
inv_cp_pos, inv_cp_rot = pb.invertTransform(child_pose.position,
child_pose.quaternion)
ZERO_VEC = (0, 0, 0)
ZERO_ROT = (0, 0, 0, 1)
pjp, pjo = pb.multiplyTransforms(inv_pp_pos, inv_pp_rot, jointPosition,
ZERO_ROT)
cjp, cjo = pb.multiplyTransforms(inv_cp_pos, inv_cp_rot, jointPosition,
ZERO_ROT)
cja, _ = pb.multiplyTransforms(ZERO_VEC, inv_cp_rot, jointAxis,
ZERO_ROT)
return self.create_constraint_local(parentBody, childBody, jointType,
parentLink, childLink, cja, pjp,
cjp, pjo, cjo, name_override)
def pose(self):
"""Returns the object's current pose in the form of a Frame.
:rtype: Frame
"""
if self.simulator.get_n_update() != self.__last_sim_pose_update:
temp = pb.getBasePositionAndOrientation(
self.__bulletId, physicsClientId=self.__client_id)
self.__current_pose = Frame(temp[0], temp[1])
return self.__current_pose
def invert_transform(frame_tuple):
"""
Inverts the transformation represented by the Frame datatype and returns it as new frame.
:type frame_tuple: iai_bullet_sim.utils.Frame
:rtype: iai_bullet_sim.utils.Frame
"""
temp = pb.invertTransform(list(frame_tuple.position),
list(frame_tuple.quaternion))
return Frame(Point3(*temp[0]), Quaternion(*temp[1]))
def get_link_state(self, link=None):
"""Returns the state of the named link.
If None is passed as link, the object's pose is returned as LinkState
:type link: str, NoneType
:rtype: LinkState
"""
if link is not None and link not in self.link_index_map:
raise Exception('Link "{}" is not defined'.format(link))
zero_vector = Vector3(0, 0, 0)
if link is None or link == self.base_link:
frame = self.pose()
return LinkState(frame, frame, frame, zero_vector, zero_vector)
else:
ls = pb.getLinkState(self.__bulletId,
self.link_index_map[link],
0,
physicsClientId=self.__client_id)
return LinkState(Frame(Vector3(*ls[0]), Quaternion(*ls[1])),
Frame(Vector3(*ls[2]), Quaternion(*ls[3])),
Frame(Vector3(*ls[4]), Quaternion(*ls[5])),
zero_vector, zero_vector)
def pose_msg_to_frame(msg):
"""Converts a geometry_msgs.msg.Pose to a Frame."""
return Frame([msg.position.x, msg.position.y, msg.position.z], [
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
])
def __init__(self, urdf_path, base_link, eef_link, wrist_link, wps, projection_frame):
self.vis = ROSVisualizer('force_test', base_frame=base_link, plot_topic='plot')
self.wps = [Frame(Vector3(*wp[3:]), Quaternion(*list(quaternion_from_rpy(*wp[:3])))) for wp in wps]
self.wpsIdx = 0
self.base_link = base_link
self.god_map = GodMap()
self.js_prefix = 'joint_state'
self.traj_pub = rospy.Publisher('~joint_trajectory', JointTrajectoryMsg, queue_size=1, tcp_nodelay=True)
self.wrench_pub = rospy.Publisher('~wrist_force_transformed', WrenchMsg, queue_size=1, tcp_nodelay=True)
# Giskard ----------------------------
# Init controller, setup controlled joints
self.controller = Controller(res_pkg_path(urdf_path), res_pkg_path('package://pbsbtest/.controllers/'), 0.6)
self.robot = self.controller.robot
self.js_input = JointStatesInput.prefix_constructor(self.god_map.get_expr, self.robot.get_joint_names(), self.js_prefix, 'position')
self.robot.set_joint_symbol_map(self.js_input)
self.controller.set_controlled_joints(self.robot.get_joint_names())
# Get eef and sensor frame
self.sensor_frame = self.robot.get_fk_expression(base_link, wrist_link)
self.eef_frame = self.robot.get_fk_expression(base_link, eef_link)
self.eef_pos = pos_of(self.eef_frame)
# Construct motion frame
mplate_pos = pos_of(self.robot.get_fk_expression(base_link, 'arm_mounting_plate'))
self.motion_frame = frame3_rpy(pi, 0, pi * 0.5, mplate_pos)
wp_frame = self.motion_frame * FrameInput.prefix_constructor('goal/position', 'goal/quaternion', self.god_map.get_expr).get_frame()
wp_pos = pos_of(wp_frame)
# Projection frame
self.world_to_projection_frame = projection_frame
# Pre init
self.god_map.set_data(['goal'], self.wps[0])
self.tick_rate = 50
deltaT = 1.0 / self.tick_rate
js = {j: SingleJointState(j) for j in self.robot.get_joint_names()}
js['gripper_base_gripper_left_joint'] = SingleJointState('gripper_base_gripper_left_joint')
self.god_map.set_data([self.js_prefix], js)
err = norm(wp_pos - self.eef_pos)
rot_err = dot(wp_frame * unitZ, self.eef_frame * unitZ)
scons = position_conv(wp_pos, self.eef_pos) # {'align position': SoftConstraint(-err, -err, 1, err)}
scons.update(rotation_conv(rot_of(wp_frame), rot_of(self.eef_frame), rot_of(self.eef_frame).subs(self.god_map.get_expr_values())))
self.controller.init(scons, self.god_map.get_free_symbols())
print('Solving for initial state...')
# Go to initial configuration
js = run_controller_until(self.god_map, self.controller, js, self.js_prefix, [(err, 0.01), (1 - rot_err, 0.001)], deltaT)[0]
print('About to plan trajectory...')
# Generate trajectory
trajectory = OrderedDict()
traj_stamp = rospy.Duration(0.0)
section_stamps = []
for x in range(1, len(self.wps)):
print('Heading for point {}'.format(x))
self.god_map.set_data(['goal'], self.wps[x])
js, sub_traj = run_controller_until(self.god_map, self.controller, js, self.js_prefix, [(err, 0.01)], deltaT)
for time_code, position in sub_traj.items():
trajectory[traj_stamp + time_code] = position
traj_stamp += sub_traj.keys()[-1]
print('Trajectory planned! {} points over a duration of {} seconds.'.format(len(trajectory), len(trajectory) * deltaT))
traj_msg = JointTrajectoryMsg()
traj_msg.header.stamp = rospy.Time.now()
traj_msg.joint_names = trajectory.items()[0][1].keys()
for t, v in trajectory.items():
point = JointTrajectoryPointMsg()
point.positions = [v[j].position for j in traj_msg.joint_names]
point.time_from_start = t
traj_msg.points.append(point)
js_advertised = False
print('Waiting for joint state topic to be published.')
while not js_advertised and not rospy.is_shutdown():
topics = [t for t, tt in rospy.get_published_topics()]
if '/joint_states' in topics:
js_advertised = True
if not js_advertised:
print('Robot does not seem to be started. Aborting...')
return
print('Joint state has been advertised. Waiting 2 seconds for the controller...')
rospy.sleep(2.0)
print('Sending trajectory.')
self.traj_pub.publish(traj_msg)
self.tfListener = tf.TransformListener()
self.wrench_sub = rospy.Subscriber('~wrist_wrench', WrenchMsg, callback=self.transform_wrench, queue_size=1)