def get_AABB(self, linkId=None):
"""Returns the bounding box of a named link.
:type linkId: str, NoneType
:rtype: AABB
"""
res = pb.getAABB(self.__bulletId,
self.link_index_map[linkId],
physicsClientId=self.__client_id)
return AABB(Vector3(*res[0]), Vector3(*res[1]))
def __create_contact_point(self, bcp):
"""Internal. Turns a bullet contact point into a ContactPoint."""
bodyA, linkA = self.__get_obj_link_tuple(bcp[1], bcp[3])
bodyB, linkB = self.__get_obj_link_tuple(bcp[2], bcp[4])
return ContactPoint(
bodyA, # Body A
bodyB, # Body B
linkA, # Link of A
linkB, # Link of B
Vector3(*bcp[5]), # Point on A
Vector3(*bcp[6]), # Point on B
Vector3(*bcp[7]), # Normal from B to A
bcp[8], # Distance
bcp[9]) # Normal force
def vec_scale(a, x):
"""
Performs per element multiplication on an indexable structure with len >= 3 and returns the result as Vector3.
:type a: iterable
:type x: iterable
:rtype: iai_bullet_sim.utils.Vector3
"""
return Vector3(a.x * x, a.y * x, a.z * x)
def vec_sub(a, b):
"""
Performs per element subtraction on two indexable structures with len >= 3 and returns the result as Vector3.
:type a: iterable
:type b: iterable
:rtype: iai_bullet_sim.utils.Vector3
"""
return Vector3(a[0] - b[0], a[1] - b[1], a[2] - b[2])
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 get_AABB(self):
"""Returns the bounding box of this object.
:rtype: AABB
"""
res = pb.getAABB(self.__bulletId, -1, physicsClientId=self.__client_id)
return AABB(Vector3(*res[0]), Vector3(*res[1]))
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)