def test_prismatic_joint(self):
ks = ArticulationModel()
a = Position('a')
b = Position('b')
c = Position('c')
parent_pose = frame3_axis_angle(vector3(0,1,0), a, point3(0, b, 5))
joint_transform = translation3(7, -5, 33)
axis = vector3(1, -3, 7)
position = c
child_pose = parent_pose * joint_transform * translation3(*(axis[:,:3] * position))
ks.apply_operation('create parent', CreateComplexObject(Path('parent'), KinematicLink('', parent_pose)))
ks.apply_operation('create child', CreateComplexObject(Path('child'), KinematicLink('', se.eye(4))))
self.assertTrue(ks.has_data('parent/pose'))
self.assertTrue(ks.has_data('child/pose'))
ks.apply_operation('connect parent child',
SetPrismaticJoint(Path('parent/pose'),
Path('child/pose'),
Path('fixed_joint'),
joint_transform,
axis,
position,
-1, 2, 0.5))
self.assertTrue(ks.has_data('fixed_joint'))
self.assertEquals(ks.get_data('child/pose'), child_pose)
def urdf_to_geometry(urdf_geom, to_modify):
to_modify.type = urdf_geom_types[type(urdf_geom)]
if to_modify.type == 'mesh':
to_modify.mesh = urdf_geom.filename
if urdf_geom.scale is not None:
to_modify.scale = vector3(*urdf_geom.scale)
elif to_modify.type == 'box':
to_modify.scale = vector3(*urdf_geom.size)
elif to_modify.type == 'cylinder':
to_modify.scale = vector3(urdf_geom.radius * 2, urdf_geom.radius * 2, urdf_geom.length)
elif to_modify.type == 'sphere':
to_modify.scale = vector3(urdf_geom.radius * 2, urdf_geom.radius * 2, urdf_geom.radius * 2)
else:
raise Exception('Can not convert geometry of type "{}"'.format(to_modify.type))
def __init__(self, robot, link, vel_limit=0.6, symmetry=None, gripper_topic=None):
self.link = link
self.fk_frame = robot.links[link].pose
self.vel_limit = vel_limit
self.pub_gcmd = rospy.Publisher(gripper_topic, GCAGMsg, queue_size=1, tcp_nodelay=True) if gripper_topic != None else None
self.last_command = None
self.current_rpy = gm.vector3(0,0,0)
self.current_lin_vel = gm.vector3(0,0,0)
self.global_command = False
self.symmetry = symmetry
self.input_lin_vel = create_symbol_vec3(f'{self.link}_lin_vel')
self.input_rot_goal = create_symbol_frame3_rpy(f'{self.link}_rot_goal')
self.input_iframe = create_symbol_frame3_rpy(f'{self.link}_iframe')
self.input_rot_offset = create_symbol_frame3_rpy(f'{self.link}_rot_offset')
def stop_motion(self, subs):
r, p, y = rpy_from_matrix(gm.subs(self.fk_frame, subs))
subs[self.input_rot_offset.rr] = r
subs[self.input_rot_offset.rp] = p
subs[self.input_rot_offset.ry] = y
subs[self.input_iframe.rr] = 0
subs[self.input_iframe.rp] = 0
subs[self.input_iframe.ry] = 0
self.current_lin_vel = gm.vector3(0,0,0)
self.current_rpy = gm.vector3(0,0,0)
subs[self.input_lin_vel.x] = 0
subs[self.input_lin_vel.y] = 0
subs[self.input_lin_vel.z] = 0
subs[self.input_rot_goal.rr] = self.current_rpy[0]
subs[self.input_rot_goal.rp] = self.current_rpy[1]
subs[self.input_rot_goal.ry] = self.current_rpy[2]
def process_input(self, subs, lx, ly, lz, ax, ay, az, oy, scale=1.0, command_type='global'):
now = rospy.Time.now()
if self.last_command is None:
rotation_matrix = gm.subs(self.fk_frame, subs)
if command_type == 'relative':
iframe = gm.rotation3_rpy(0,0, oy)
subs[self.input_iframe.rr] = 0
subs[self.input_iframe.rp] = 0
subs[self.input_iframe.ry] = oy
elif command_type == 'global':
iframe = gm.eye(4)
subs[self.input_iframe.rr] = 0
subs[self.input_iframe.rp] = 0
subs[self.input_iframe.ry] = 0
else:
iframe = rotation_matrix
if self.symmetry == 'xz' and rotation_matrix[2,2] < 0:
iframe = gm.rotation3_rpy(math.pi, 0, 0) * iframe
r, p, y = rpy_from_matrix(iframe)
subs[self.input_iframe.rr] = r
subs[self.input_iframe.rp] = p
subs[self.input_iframe.ry] = y
r, p, y = rpy_from_matrix(iframe.T * rotation_matrix)
subs[self.input_rot_offset.rr] = r
subs[self.input_rot_offset.rp] = p
subs[self.input_rot_offset.ry] = y
self.last_command = now
self.command_type = command_type
self.current_rpy = gm.vector3(ax, ay, az)
self.current_lin_vel = gm.vector3(lx, ly, lz) * self.vel_limit * scale
subs[self.input_lin_vel.x] = self.current_lin_vel[0]
subs[self.input_lin_vel.y] = self.current_lin_vel[1]
subs[self.input_lin_vel.z] = self.current_lin_vel[2]
subs[self.input_rot_goal.rr] = self.current_rpy[0]
subs[self.input_rot_goal.rp] = self.current_rpy[1]
subs[self.input_rot_goal.ry] = self.current_rpy[2]
def test_revolute_and_continuous_joint(self):
ks = ArticulationModel()
a = Position('a')
b = Position('b')
c = Position('c')
parent_pose = frame3_axis_angle(vector3(0,1,0), a, point3(0, b, 5))
joint_transform = translation3(7, -5, 33)
axis = vector3(1, -3, 7)
axis = axis / norm(axis)
position = c
child_pose = parent_pose * joint_transform * rotation3_axis_angle(axis, position)
ks.apply_operation('create parent', CreateComplexObject(Path('parent'), KinematicLink('', parent_pose)))
ks.apply_operation('create child', CreateComplexObject(Path('child'), KinematicLink('', se.eye(4))))
self.assertTrue(ks.has_data('parent/pose'))
self.assertTrue(ks.has_data('child/pose'))
ks.apply_operation('connect parent child',
SetRevoluteJoint(Path('parent/pose'),
Path('child/pose'),
Path('fixed_joint'),
joint_transform,
axis,
position,
-1, 2, 0.5))
self.assertTrue(ks.has_data('fixed_joint'))
self.assertEquals(ks.get_data('child/pose'), child_pose)
ks.remove_operation('connect parent child')
ks.apply_operation('connect parent child',
SetContinuousJoint(Path('parent/pose'),
Path('child/pose'),
Path('fixed_joint'),
joint_transform,
axis,
position, 0.5))
def __init__(self, km, controlled_symbols, resting_pose, camera_path=None):
tucking_constraints = {}
if resting_pose is not None:
tucking_constraints = {
f'tuck {s}': SC(p - s, p - s, 1, s)
for s, p in resting_pose.items()
}
# print('Tuck state:\n {}\nTucking constraints:\n {}'.format('\n '.join(['{}: {}'.format(k, v) for k, v in self._resting_pose.items()]), '\n '.join(tucking_constraints.keys())))
# tucking_constraints.update(self.taxi_constraints)
self.use_camera = camera_path is not None
if camera_path is not None:
self._poi_pos = gm.Symbol('poi')
poi = gm.point3(1.5, 0.5, 0.0) + gm.vector3(
0, self._poi_pos * 2.0, 0)
camera = km.get_data(camera_path)
cam_to_poi = poi - gm.pos_of(camera.pose)
lookat_dot = 1 - gm.dot_product(gm.x_of(camera.pose),
cam_to_poi) / gm.norm(cam_to_poi)
tucking_constraints['sweeping gaze'] = SC(-lookat_dot * 5,
-lookat_dot * 5, 1,
lookat_dot)
symbols = set()
for c in tucking_constraints.values():
symbols |= gm.free_symbols(c.expr)
joint_symbols = {
s
for s in symbols if gm.get_symbol_type(s) != gm.TYPE_UNKNOWN
}
controlled_symbols = {gm.DiffSymbol(s) for s in joint_symbols}
hard_constraints = km.get_constraints_by_symbols(
symbols.union(controlled_symbols))
controlled_values, hard_constraints = generate_controlled_values(
hard_constraints, controlled_symbols)
controlled_values = depth_weight_controlled_values(
km, controlled_values)
self.qp = TQPB(hard_constraints, tucking_constraints,
controlled_values)
self._start = Time.now()
def test_fixed_joint(self):
ks = ArticulationModel()
a = Position('a')
b = Position('b')
parent_pose = frame3_axis_angle(vector3(0,1,0), a, point3(0, b, 5))
joint_transform = translation3(7, -5, 33)
child_pose = parent_pose * joint_transform
ks.apply_operation('create parent', CreateComplexObject(Path('parent'), KinematicLink('', parent_pose)))
ks.apply_operation('create child', CreateComplexObject(Path('child'), KinematicLink('', se.eye(4))))
self.assertTrue(ks.has_data('parent/pose'))
self.assertTrue(ks.has_data('child/pose'))
ks.apply_operation('connect parent child', SetFixedJoint(Path('parent/pose'), Path('child/pose'), Path('fixed_joint'), joint_transform))
self.assertTrue(ks.has_data('fixed_joint'))
self.assertEquals(ks.get_data('child/pose'), child_pose)
def insert_omni_base(km, robot_path, root_link, world_frame='world', lin_vel=1.0, ang_vel=0.6):
if not km.has_data(world_frame):
km.apply_operation_before(f'create {world_frame}', f'create {robot_path}', ExecFunction(Path(world_frame), Frame, ''))
base_joint_path = robot_path + Path(f'joints/to_{world_frame}')
base_op = ExecFunction(base_joint_path,
create_omnibase_joint_with_symbols,
CPath(f'{world_frame}/pose'),
CPath(robot_path + Path(f'links/{root_link}/pose')),
gm.vector3(0,0,1),
1.0, 0.6, CPath(robot_path))
km.apply_operation_after(f'create {base_joint_path}',
f'create {robot_path}/links/{root_link}', base_op)
km.apply_operation_after(f'connect {world_frame} {robot_path}/links/{root_link}',
f'create {base_joint_path}',
CreateAdvancedFrameConnection(base_joint_path,
Path(world_frame),
robot_path + Path(f'links/{root_link}')))
km.clean_structure()
def track(self, model_path, alias):
with self.lock:
if type(model_path) is not str:
model_path = str(model_path)
start_tick = len(self.tracked_poses) == 0 and self._use_timer
if model_path not in self.tracked_poses:
syms = [
Position(Path(model_path) + (x, ))
for x in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']
]
def rf(msg, state):
state[syms[0]] = msg.position.x
state[syms[1]] = msg.position.y
state[syms[2]] = msg.position.z
state[syms[3]] = msg.orientation.x
state[syms[4]] = msg.orientation.y
state[syms[5]] = msg.orientation.z
state[syms[6]] = msg.orientation.w
def process_model_update(data):
self._generate_pose_constraints(model_path, data)
axis = vector3(*syms[:3])
self.aliases[alias] = model_path
self.tracked_poses[model_path] = TrackerEntry(
frame3_quaternion(*syms), rf, process_model_update)
self._unintialized_poses.add(model_path)
if type(self.km_client) == ModelClient:
self.km_client.register_on_model_changed(
Path(model_path), process_model_update)
else:
process_model_update(self.km_client.get_data(model_path))
if start_tick:
self.timer = rospy.Timer(rospy.Duration(1.0 / 50),
self.cb_tick)
urdf_model = URDF.from_xml_string(
rospy.get_param('/robot_description'))
else:
urdf_model = URDF.from_xml_file(res_pkg_path(sys.argv[1]))
# Fill collision geometry of demanded
if len(sys.argv) >= 3 and (sys.argv[2].lower() == 'true'
or sys.argv[2] == '1'):
urdf_model = urdf_filler(urdf_model)
op_client = OperationsClient(EventModel, True)
load_urdf(op_client, urdf_model.name, urdf_model)
op_client.apply_operation_before(
'create world', 'create {}'.format(urdf_model.name),
CreateComplexObject(Path('world'), Frame('')))
omni_op = create_omnibase_joint_with_symbols(
Path('world/pose'),
Path('{}/links/{}/pose'.format(urdf_model.name,
urdf_model.get_root())),
Path('{}/joints/to_world'.format(urdf_model.name)), vector3(0, 0, 1),
1.0, 0.6, Path(urdf_model.name))
op_client.apply_operation_after(
'connect world {}'.format(urdf_model.get_root()),
'create {}/{}'.format(urdf_model.name, urdf_model.get_root()), omni_op)
op_client.apply_changes()
op_client.kill()
def create_symbol_vec3(prefix):
if not isinstance(prefix, Path):
prefix = Path(prefix)
symbols = [p.to_symbol() for p in [prefix + (x,) for x in 'xyz']]
return SymbolVector3(gm.vector3(*symbols), *symbols)
fac = (stamp - p_data.stamp).to_sec() / segment_length
return (1 - fac) * getattr(p_data, value) + fac * getattr(data, value)
angle_hinge = gm.Symbol('angle_top')
x_hinge_in_parent, z_hinge_in_parent = [
gm.Symbol(f'hinge_in_parent_{x}') for x in 'xz'
]
x_child_in_hinge, z_child_in_hinge = [
gm.Symbol(f'child_in_hinge_{x}') for x in 'xz'
]
fwd_kinematic_hinge = gm.dot(
gm.translation3(x_hinge_in_parent, 0, z_hinge_in_parent),
gm.rotation3_axis_angle(gm.vector3(0, -1, 0), angle_hinge),
gm.translation3(x_child_in_hinge, 0, z_child_in_hinge))
# we don't care about the location in y
fwd_kinematic_hinge_residual_tf = gm.speed_up(
gm.dot(gm.diag(1, 0, 1, 1), fwd_kinematic_hinge),
gm.free_symbols(fwd_kinematic_hinge))
def compute_y_hinge_axis_residual(x, angle_tf):
"""Computes the residual for an estimate of the hinge locations on the nobilia shelf
Args:
x (np.ndarray): [xh, zh, xc, zc] x and z locations of hinge in parent and child in hinge
angle_1_tf (TYPE): list of tuples (a, tf) where a is the angle of the top panel relative to the parent
"""
param_dict = {
def create_nobilia_shelf(km,
prefix,
origin_pose=gm.eye(4),
parent_path=Path('world')):
km.apply_operation(
f'create {prefix}',
ExecFunction(prefix, MarkedArticulatedObject, str(prefix)))
shelf_height = 0.72
shelf_width = 0.6
shelf_body_depth = 0.35
wall_width = 0.016
l_prefix = prefix + ('links', )
geom_body_wall_l = Box(
l_prefix + ('body', ),
gm.translation3(0, 0.5 * (shelf_width - wall_width), 0),
gm.vector3(shelf_body_depth, wall_width, shelf_height))
geom_body_wall_r = Box(
l_prefix + ('body', ),
gm.translation3(0, -0.5 * (shelf_width - wall_width), 0),
gm.vector3(shelf_body_depth, wall_width, shelf_height))
geom_body_ceiling = Box(
l_prefix + ('body', ),
gm.translation3(0, 0, 0.5 * (shelf_height - wall_width)),
gm.vector3(shelf_body_depth, shelf_width - wall_width, wall_width))
geom_body_floor = Box(
l_prefix + ('body', ),
gm.translation3(0, 0, -0.5 * (shelf_height - wall_width)),
gm.vector3(shelf_body_depth, shelf_width - wall_width, wall_width))
geom_body_shelf_1 = Box(
l_prefix + ('body', ),
gm.translation3(0.02, 0, -0.2 * (shelf_height - wall_width)),
gm.vector3(shelf_body_depth - 0.04, shelf_width - wall_width,
wall_width))
geom_body_shelf_2 = Box(
l_prefix + ('body', ),
gm.translation3(0.02, 0, 0.2 * (shelf_height - wall_width)),
gm.vector3(shelf_body_depth - 0.04, shelf_width - wall_width,
wall_width))
geom_body_back = Box(
l_prefix + ('body', ),
gm.translation3(0.5 * (shelf_body_depth - 0.005), 0, 0),
gm.vector3(0.005, shelf_width - 2 * wall_width,
shelf_height - 2 * wall_width))
shelf_geom = [
geom_body_wall_l, geom_body_wall_r, geom_body_ceiling, geom_body_floor,
geom_body_back, geom_body_shelf_1, geom_body_shelf_2
]
rb_body = RigidBody(parent_path,
origin_pose,
geometry=dict(enumerate(shelf_geom)),
collision=dict(enumerate(shelf_geom)))
geom_panel_top = Box(l_prefix + ('panel_top', ), gm.eye(4),
gm.vector3(0.357, 0.595, wall_width))
geom_panel_bottom = Box(l_prefix + ('panel_bottom', ), gm.eye(4),
gm.vector3(0.357, 0.595, wall_width))
handle_width = 0.16
handle_depth = 0.05
handle_diameter = 0.012
geom_handle_r = Box(
l_prefix + ('handle', ),
gm.translation3(0.5 * handle_depth,
0.5 * (handle_width - handle_diameter), 0),
gm.vector3(handle_depth, handle_diameter, handle_diameter))
geom_handle_l = Box(
l_prefix + ('handle', ),
gm.translation3(0.5 * handle_depth,
-0.5 * (handle_width - handle_diameter), 0),
gm.vector3(handle_depth, handle_diameter, handle_diameter))
geom_handle_bar = Box(
l_prefix + ('handle', ),
gm.translation3(handle_depth - 0.5 * handle_diameter, 0, 0),
gm.vector3(handle_diameter, handle_width - handle_diameter,
handle_diameter))
handle_geom = [geom_handle_l, geom_handle_r, geom_handle_bar]
# Sketch of mechanism
#
# T ---- a
# ---- \ Z
# b ..... V \
# | ...... d
# B | ------
# c ------
# L
#
# Diagonal V is virtual
#
#
# Angles:
# a -> alpha (given)
# b -> gamma_1 + gamma_2 = gamma
# c -> don't care
# d -> delta_1 + delta_2 = delta
#
opening_position = gm.Position(prefix + ('door', ))
# Calibration results
#
# Solution top hinge: cost = 0.03709980624159568 [ 0.08762252 -0.01433833 0.2858676 0.00871125]
# Solution bottom hinge: cost = 0.025004236048128934 [ 0.1072496 -0.01232362 0.27271013 0.00489996]
# Added 180 deg rotation due to -x being the forward facing side in this model
top_hinge_in_body_marker = gm.translation3(0.08762252 - 0.015, 0,
-0.01433833)
top_panel_marker_in_top_hinge = gm.translation3(0.2858676 - 0.003,
-wall_width + 0.0025,
0.00871125 - 0.003)
front_hinge_in_top_panel_maker = gm.translation3(0.1072496 - 0.02, 0,
-0.01232362 + 0.007)
bottom_panel_marker_in_front_hinge = gm.translation3(
0.27271013, 0, 0.00489996)
# Top hinge - Data taken from observation
body_marker_in_body = gm.dot(
gm.rotation3_axis_angle(gm.vector3(0, 0, 1), math.pi),
gm.translation3(0.5 * shelf_body_depth - 0.062,
-0.5 * shelf_width + 0.078, 0.5 * shelf_height))
top_panel_marker_in_top_panel = gm.translation3(
geom_panel_top.scale[0] * 0.5 - 0.062,
-geom_panel_top.scale[1] * 0.5 + 0.062, geom_panel_top.scale[2] * 0.5)
bottom_panel_marker_in_bottom_panel = gm.translation3(
geom_panel_bottom.scale[0] * 0.5 - 0.062,
-geom_panel_bottom.scale[1] * 0.5 + 0.062,
geom_panel_bottom.scale[2] * 0.5)
top_hinge_in_body = gm.dot(body_marker_in_body, top_hinge_in_body_marker)
top_panel_in_top_hinge = gm.dot(
top_panel_marker_in_top_hinge,
gm.inverse_frame(top_panel_marker_in_top_panel))
front_hinge_in_top_panel = gm.dot(top_panel_marker_in_top_panel,
front_hinge_in_top_panel_maker)
bottom_panel_in_front_hinge = gm.dot(
bottom_panel_marker_in_front_hinge,
gm.inverse_frame(bottom_panel_marker_in_bottom_panel))
# Point a in body reference frame
point_a = gm.dot(gm.diag(1, 0, 1, 1), gm.pos_of(top_hinge_in_body))
point_d = gm.point3(-shelf_body_depth * 0.5 + 0.09, 0,
shelf_height * 0.5 - 0.192)
# point_d = gm.point3(-shelf_body_depth * 0.5 + gm.Symbol('point_d_x'), 0, shelf_height * 0.5 - gm.Symbol('point_d_z'))
# Zero alpha along the vertical axis
vec_a_to_d = gm.dot(point_d - point_a)
alpha = gm.atan2(vec_a_to_d[0], -vec_a_to_d[2]) + opening_position
top_panel_in_body = gm.dot(
top_hinge_in_body, # Translation hinge to body frame
gm.rotation3_axis_angle(gm.vector3(0, 1, 0), -opening_position +
0.5 * math.pi), # Hinge around y
top_panel_in_top_hinge)
front_hinge_in_body = gm.dot(top_panel_in_body, front_hinge_in_top_panel)
# Point b in top panel reference frame
point_b_in_top_hinge = gm.pos_of(
gm.dot(gm.diag(1, 0, 1, 1), front_hinge_in_top_panel,
top_panel_in_top_hinge))
point_b = gm.dot(gm.diag(1, 0, 1, 1), gm.pos_of(front_hinge_in_body))
# Hinge lift arm in body reference frame
point_c_in_bottom_panel = gm.dot(
gm.diag(1, 0, 1, 1),
bottom_panel_marker_in_bottom_panel,
gm.point3(-0.094, -0.034, -0.072),
# gm.point3(-gm.Symbol('point_c_x'), -0.034, -gm.Symbol('point_c_z'))
)
point_c_in_front_hinge = gm.dot(
gm.diag(1, 0, 1, 1),
gm.dot(bottom_panel_in_front_hinge, point_c_in_bottom_panel))
length_z = gm.norm(point_a - point_d)
vec_a_to_b = point_b - point_a
length_t = gm.norm(vec_a_to_b)
length_b = gm.norm(point_c_in_front_hinge[:3])
# length_l = gm.Symbol('length_l') # 0.34
length_l = 0.372
vec_b_to_d = point_d - point_b
length_v = gm.norm(vec_b_to_d)
gamma_1 = inner_triangle_angle(length_t, length_v, length_z)
gamma_2 = inner_triangle_angle(length_b, length_v, length_l)
top_panel_offset_angle = gm.atan2(point_b_in_top_hinge[2],
point_b_in_top_hinge[0])
bottom_offset_angle = gm.atan2(point_c_in_front_hinge[2],
point_c_in_front_hinge[0])
gamma = gamma_1 + gamma_2
rb_panel_top = RigidBody(l_prefix + ('body', ),
gm.dot(rb_body.pose, top_panel_in_body),
top_panel_in_body,
geometry={0: geom_panel_top},
collision={0: geom_panel_top})
# old offset: 0.5 * geom_panel_top.scale[2] + 0.03
tf_bottom_panel = gm.dot(
front_hinge_in_top_panel,
gm.rotation3_axis_angle(
gm.vector3(0, 1, 0),
math.pi + bottom_offset_angle - top_panel_offset_angle),
gm.rotation3_axis_angle(gm.vector3(0, -1, 0), gamma),
bottom_panel_in_front_hinge)
rb_panel_bottom = RigidBody(l_prefix + ('panel_top', ),
gm.dot(rb_panel_top.pose, tf_bottom_panel),
tf_bottom_panel,
geometry={0: geom_panel_bottom},
collision={0: geom_panel_bottom})
handle_transform = gm.dot(
gm.translation3(geom_panel_bottom.scale[0] * 0.5 - 0.08, 0,
0.5 * wall_width),
gm.rotation3_axis_angle(gm.vector3(0, 1, 0), -math.pi * 0.5))
rb_handle = RigidBody(l_prefix + ('panel_bottom', ),
gm.dot(rb_panel_bottom.pose, handle_transform),
handle_transform,
geometry={x: g
for x, g in enumerate(handle_geom)},
collision={x: g
for x, g in enumerate(handle_geom)})
# Only debugging
point_c = gm.dot(rb_panel_bottom.pose, point_c_in_bottom_panel)
vec_b_to_c = point_c - point_b
km.apply_operation(f'create {prefix}/links/body',
CreateValue(rb_panel_top.parent, rb_body))
km.apply_operation(f'create {prefix}/links/panel_top',
CreateValue(rb_panel_bottom.parent, rb_panel_top))
km.apply_operation(
f'create {prefix}/links/panel_bottom',
CreateValue(l_prefix + ('panel_bottom', ), rb_panel_bottom))
km.apply_operation(f'create {prefix}/links/handle',
CreateValue(l_prefix + ('handle', ), rb_handle))
km.apply_operation(
f'create {prefix}/joints/hinge',
ExecFunction(
prefix + Path('joints/hinge'), RevoluteJoint,
CPath(rb_panel_top.parent), CPath(rb_panel_bottom.parent),
opening_position, gm.vector3(0, 1, 0), gm.eye(4), 0, 1.84, **{
f'{opening_position}':
Constraint(0 - opening_position, 1.84 - opening_position,
opening_position),
f'{gm.DiffSymbol(opening_position)}':
Constraint(-0.25, 0.25, gm.DiffSymbol(opening_position))
}))
m_prefix = prefix + ('markers', )
km.apply_operation(
f'create {prefix}/markers/body',
ExecFunction(m_prefix + ('body', ), Frame,
CPath(l_prefix + ('body', )),
gm.dot(rb_body.pose,
body_marker_in_body), body_marker_in_body))
km.apply_operation(
f'create {prefix}/markers/top_panel',
ExecFunction(m_prefix + ('top_panel', ), Frame,
CPath(l_prefix + ('panel_top', )),
gm.dot(rb_panel_top.pose, top_panel_marker_in_top_panel),
top_panel_marker_in_top_panel))
km.apply_operation(
f'create {prefix}/markers/bottom_panel',
ExecFunction(
m_prefix + ('bottom_panel', ), Frame,
CPath(l_prefix + ('panel_bottom', )),
gm.dot(rb_panel_bottom.pose, bottom_panel_marker_in_bottom_panel),
bottom_panel_marker_in_bottom_panel))
return NobiliaDebug(
[
top_hinge_in_body,
gm.dot(
top_hinge_in_body,
gm.rotation3_axis_angle(gm.vector3(0, 1, 0),
-opening_position + 0.5 * math.pi),
top_panel_in_top_hinge, front_hinge_in_top_panel),
body_marker_in_body,
gm.dot(rb_panel_top.pose, top_panel_marker_in_top_panel),
gm.dot(rb_panel_bottom.pose, bottom_panel_marker_in_bottom_panel)
], [(point_a, vec_a_to_d), (point_a, vec_a_to_b),
(point_b, vec_b_to_d), (point_b, vec_b_to_c)],
{
'gamma_1':
gamma_1,
'gamma_1 check_dot':
gamma_1 - gm.acos(
gm.dot_product(-vec_a_to_b / gm.norm(vec_a_to_b),
vec_b_to_d / gm.norm(vec_b_to_d))),
'gamma_1 check_cos':
gamma_1 - inner_triangle_angle(
gm.norm(vec_a_to_b), gm.norm(vec_b_to_d), gm.norm(vec_a_to_d)),
'gamma_2':
gamma_2,
'gamma_2 check_dot':
gamma_2 - gm.acos(
gm.dot_product(vec_b_to_c / gm.norm(vec_b_to_c),
vec_b_to_d / gm.norm(vec_b_to_d))),
'length_v':
length_v,
'length_b':
length_b,
'length_l':
length_l,
'position':
opening_position,
'alpha':
alpha,
'dist c d':
gm.norm(point_d - point_c)
}, {
gm.Symbol('point_c_x'): 0.094,
gm.Symbol('point_c_z'): 0.072,
gm.Symbol('point_d_x'): 0.09,
gm.Symbol('point_d_z'): 0.192,
gm.Symbol('length_l'): 0.372
})
goal_grasp_lin = SoftConstraint(-grasp_err_lin, -grasp_err_lin,
100.0, grasp_err_lin)
goal_grasp_ang = SoftConstraint(-grasp_err_rot, -grasp_err_rot,
10.0, grasp_err_rot)
goal_door_angle = SoftConstraint(math.pi * 0.45 - door_position,
math.pi * 0.45 - door_position,
1.0, door_position)
goal_handle_angle = SoftConstraint(
math.pi * 0.25 - handle_position,
math.pi * 0.25 - handle_position, 1.0, handle_position)
# Dynamic grasp goal
ordered_active_symbols = list(active_symbols)
goal_lin_vel = gm.vector3(
gm.get_diff(gm.pos_of(goal_pose)[0], controlled_symbols),
gm.get_diff(gm.pos_of(goal_pose)[1], controlled_symbols),
gm.get_diff(gm.pos_of(goal_pose)[2], controlled_symbols))
eef_lin_vel = gm.vector3(
gm.get_diff(gm.pos_of(eef.pose)[0], controlled_symbols),
gm.get_diff(gm.pos_of(eef.pose)[1], controlled_symbols),
gm.get_diff(gm.pos_of(eef.pose)[2], controlled_symbols))
lin_vel_alignment = gm.dot_product(goal_lin_vel, eef_lin_vel)
goal_lin_vel_sq_norm = gm.dot(eef_lin_vel.T, eef_lin_vel)
goal_lin_vel_alignment = SoftConstraint(
goal_lin_vel_sq_norm - lin_vel_alignment,
goal_lin_vel_sq_norm - lin_vel_alignment, 2, lin_vel_alignment)
qp = GQPB(
world,
def generate_push_closing(km,
grounding_state,
controlled_symbols,
eef_pose,
obj_pose,
eef_path,
obj_path,
nav_method='cross',
cp_offset=0,
static_symbols=set()):
# CONTACT GEOMETRY
robot_cp, object_cp, contact_normal = contact_geometry(
eef_pose, obj_pose, eef_path, obj_path)
object_cp = object_cp - contact_normal * cp_offset
geom_distance = gm.dot_product(contact_normal, robot_cp - object_cp)
coll_world = km.get_active_geometry(gm.free_symbols(geom_distance))
# GEOMETRY NAVIGATION LOGIC
# This is exploiting task knowledge which makes this function inflexible.
contact_grad = sum([
sign(-grounding_state[s]) *
gm.vector3(gm.diff(object_cp[0], s), gm.diff(object_cp[1], s),
gm.diff(object_cp[2], s))
for s in gm.free_symbols(obj_pose) if s not in static_symbols
], gm.vector3(0, 0, 0))
neutral_tangent = gm.cross(contact_grad, contact_normal)
active_tangent = gm.cross(neutral_tangent, contact_normal)
contact_constraints, in_contact = generate_contact_model(
robot_cp, controlled_symbols, object_cp, contact_normal,
gm.free_symbols(obj_pose))
target_pos = None
if nav_method == 'linear':
geom_distance = gm.norm(object_cp + active_tangent * geom_distance +
contact_grad * 0.05 - robot_cp)
elif nav_method == 'cubic':
dist_scaling = 2**(-0.5 * ((geom_distance - 0.2) / (0.2 * 0.2))**2)
geom_distance = gm.norm(object_cp + active_tangent * dist_scaling -
robot_cp)
elif nav_method == 'cross':
geom_distance = gm.norm(object_cp +
active_tangent * gm.norm(neutral_tangent) +
contact_grad * 0.05 - robot_cp)
elif nav_method == 'cross_deep':
geom_distance = gm.norm(object_cp +
active_tangent * gm.norm(neutral_tangent) +
contact_grad *
-gm.dot_product(contact_normal, contact_grad) -
robot_cp)
elif nav_method == 'none' or nav_method is None:
pass
elif nav_method == 'proj':
obj_cp_dist = gm.dot_product(contact_normal,
object_cp - gm.pos_of(obj_pose))
target_pos = gm.pos_of(
obj_pose
) + contact_normal * obj_cp_dist - contact_normal * 0.02 # Drive into the surface
geom_distance = gm.norm(robot_cp - target_pos)
contact_relative_pos = gm.dot(gm.rot_of(obj_pose),
robot_cp - gm.pos_of(obj_pose))
contact_relative_vel = gm.vector3(
sum([gm.diff(contact_relative_pos[0], s) for s in controlled_symbols],
0),
sum([gm.diff(contact_relative_pos[1], s) for s in controlled_symbols],
0),
sum([gm.diff(contact_relative_pos[2], s) for s in controlled_symbols],
0))
# PUSH CONSTRAINT GENERATION
constraints = km.get_constraints_by_symbols(
gm.free_symbols(geom_distance).union(controlled_symbols))
constraints.update(contact_constraints)
# for x, n in enumerate('xyz'):
# constraints[f'zero tangent vel_{n}'] = Constraint((1 - in_contact) * -1e3,
# (1 - in_contact) * 1e3, contact_relative_pos[x] * (1.0 + 0.1 * x))
def debug_draw(vis, state, cmd):
vis.begin_draw_cycle('debug_vecs')
s_object_cp = gm.subs(object_cp, state)
s_neutral_tangent = gm.subs(neutral_tangent, state)
vis.draw_vector('debug_vecs',
s_object_cp,
gm.subs(contact_grad, state),
r=0,
b=0)
vis.draw_vector('debug_vecs',
s_object_cp,
gm.subs(active_tangent, state),
r=0,
b=1)
vis.draw_vector('debug_vecs',
s_object_cp,
gm.subs(neutral_tangent, state),
r=1,
g=1,
b=0)
if target_pos is not None:
vis.draw_sphere('debug_vecs',
gm.subs(target_pos, state),
0.01,
r=0,
b=1)
# print(f'{gm.norm(gm.subs(contact_normal, state))}')
# vis.draw_vector('debug_vecs', s_object_cp, s_ortho_vel_vec, r=1, b=0)
vis.render('debug_vecs')
return constraints, geom_distance, coll_world, PushingInternals(
robot_cp, contact_normal, contact_relative_pos, contact_relative_vel,
debug_draw)
def __init__(self,
km,
actuated_object_path,
target_object_path,
controlled_symbols,
start_state,
camera_path=None,
navigation_method='cross',
visualizer=None,
weight_override=None,
collision_avoidance_paths=None):
print(f'{actuated_object_path}\n{target_object_path}')
actuated_object = km.get_data(actuated_object_path)
target_object = km.get_data(target_object_path)
all_controlled_symbols = controlled_symbols.union({
gm.DiffSymbol(j)
for j in gm.free_symbols(target_object.pose)
if 'location' not in str(j)
})
static_symbols = {
s
for s in gm.free_symbols(target_object.pose)
if 'location' in str(s)
}
# Generate push problem
constraints, \
geom_distance, \
coll_world, \
self.p_internals = generate_push_closing(km,
start_state,
all_controlled_symbols,
actuated_object.pose,
target_object.pose,
actuated_object_path,
target_object_path,
navigation_method,
static_symbols=static_symbols)
start_state.update({s: 0.0 for s in gm.free_symbols(coll_world)})
weight_override = {} if weight_override is None else weight_override
controlled_values, \
constraints = generate_controlled_values(constraints,
all_controlled_symbols)
controlled_values = depth_weight_controlled_values(km,
controlled_values,
exp_factor=1.1)
goal_constraints = {
'reach_point': PIDC(geom_distance, geom_distance, 1, k_i=0.00)
}
for s, w in weight_override.items():
for cv in controlled_values.values():
if cv.symbol is s:
cv.weight_id = w
break
# CAMERA STUFF
if camera_path is not None:
camera = km.get_data(camera_path)
cam_pos = gm.pos_of(camera.pose)
cam_to_obj = gm.pos_of(target_object.pose) - cam_pos
cam_forward = gm.dot(camera.pose, gm.vector3(1, 0, 0))
look_goal = 1 - (gm.dot_product(cam_to_obj, cam_forward) /
gm.norm(cam_to_obj))
goal_constraints['look_at_obj'] = SC(-look_goal, -look_goal, 1,
look_goal)
# GOAL CONSTAINT GENERATION
# 'avoid_collisions': SC.from_constraint(closest_distance_constraint_world(eef_pose, eef_path[:-1], 0.03), 100)
# }
if collision_avoidance_paths is not None:
for p in collision_avoidance_paths:
obj = km.get_data(p)
goal_constraints[f'avoid_collision {p}'] = SC.from_constraint(
closest_distance_constraint(actuated_object.pose, obj.pose,
actuated_object_path, p, 0.01),
100)
goal_constraints.update({
f'open_object_{x}': PIDC(s, s, 1)
for x, s in enumerate(gm.free_symbols(target_object.pose))
})
self.look_goal = look_goal if camera_path is not None else None
self.in_contact = gm.less_than(geom_distance, 0.01)
self.controlled_values = controlled_values
self.geom_distance = geom_distance
self.qpb = GQPB(coll_world,
constraints,
goal_constraints,
controlled_values,
visualizer=visualizer)
self.qpb._cb_draw = self.p_internals.f_debug_draw
def create_door(km,
prefix,
height,
width,
frame_width=0.05,
to_world_tf=gm.eye(4)):
km.apply_operation(f'create {prefix}',
ExecFunction(prefix, ArticulatedObject, 'door'))
prefix = prefix + ('links', )
base_plate_geom = Box(prefix + ('frame', ), gm.translation3(0, 0, 0.015),
gm.vector3(0.2, width + 0.2, 0.03))
frame_pillar_l_geom = Box(
prefix + ('frame', ),
gm.translation3(0, 0.5 * (width + frame_width), 0.5 * height + 0.03),
gm.vector3(frame_width, frame_width, height))
frame_pillar_r_geom = Box(
prefix + ('frame', ),
gm.translation3(0, -0.5 * (width + frame_width), 0.5 * height + 0.03),
gm.vector3(frame_width, frame_width, height))
frame_bar_geom = Box(
prefix + ('frame', ),
gm.translation3(0, 0, height + 0.5 * frame_width + 0.03),
gm.vector3(frame_width, width + 2 * frame_width, frame_width))
frame_rb = RigidBody(Path('world'),
to_world_tf,
geometry={
1: base_plate_geom,
2: frame_pillar_l_geom,
3: frame_pillar_r_geom,
4: frame_bar_geom
},
collision={
1: base_plate_geom,
2: frame_pillar_l_geom,
3: frame_pillar_r_geom,
4: frame_bar_geom
})
door_geom1 = Box(prefix + ('door', ), gm.translation3(0.015, 0, 0),
gm.vector3(0.03, width, height))
door_geom2 = Box(prefix + ('door', ), gm.translation3(-0.005, 0, 0.01),
gm.vector3(0.01, width + 0.02, height + 0.01))
handle_bar_geom = Box(prefix + ('handle', ),
gm.translation3(-0.08, 0.06, 0),
gm.vector3(0.02, 0.12, 0.02))
handle_cylinder_geom = Cylinder(
prefix + ('handle', ),
gm.dot(gm.translation3(-0.04, 0, 0),
gm.rotation3_axis_angle(gm.vector3(0, 1, 0), 0.5 * math.pi)),
0.02, 0.08)
door_rb = RigidBody(Path(f'{prefix}/frame'),
gm.translation3(0.0, 0.5 * -width - 0.01, 0),
geometry={
1: door_geom1,
2: door_geom2
},
collision={
1: door_geom1,
2: door_geom2
})
handle_rb = RigidBody(Path(f'{prefix}/door'),
gm.eye(4),
geometry={
1: handle_bar_geom,
2: handle_cylinder_geom
},
collision={
1: handle_bar_geom,
2: handle_cylinder_geom
})
km.apply_operation(f'create {prefix}/frame',
CreateValue(prefix + ('frame', ), frame_rb))
km.apply_operation(f'create {prefix}/door',
CreateValue(prefix + ('door', ), door_rb))
km.apply_operation(f'create {prefix}/handle',
CreateValue(prefix + ('handle', ), handle_rb))
door_position = gm.Position('door')
handle_position = gm.Position('handle')
prefix = prefix[:-1] + ('joints', )
km.apply_operation(
f'create {prefix}',
ExecFunction(
prefix + ('hinge', ), RevoluteJoint, CPath(door_rb.parent),
CPath(handle_rb.parent), door_position, gm.vector3(0, 0, -1),
gm.translation3(0.5 * -frame_width - 0.005, 0.5 * width + 0.01,
0.5 * height + 0.03), 0, 0.75 * math.pi, 100, 1,
0))
km.apply_operation(
f'create {prefix}',
ExecFunction(prefix + ('handle', ), RevoluteJoint,
CPath(handle_rb.parent), CPath(f'{prefix}/handle'),
handle_position, gm.vector3(-1, 0, 0),
gm.translation3(0, -0.5 * width - 0.02 + 0.06,
0), 0, 0.25 * math.pi, 100, 1, 0))
prefix = prefix[:-1]
km.apply_operation(
f'connect {prefix}/links/frame {prefix}/links/door',
CreateURDFFrameConnection(prefix + ('joints', 'hinge'),
Path(door_rb.parent),
Path(handle_rb.parent)))
km.apply_operation(
f'connect {prefix}/links/door {prefix}/links/handle',
CreateURDFFrameConnection(prefix + ('joints', 'handle'),
Path(handle_rb.parent),
Path(f'{prefix}/links/handle')))
km.apply_operation(
f'add lock {door_position}',
ConditionalDoorHandleConstraints(door_position, handle_position,
math.pi * 0.01, math.pi * 0.15))
def urdf_axis_to_vector(axis):
return vector3(*axis) if axis is not None else vector3(1.0,0.0,0.0)
import kineverse.gradients.gradient_math as gm
import numpy as np
import matplotlib.pyplot as plt
from kineverse.visualization.plotting import hsv_to_rgb, \
rgb_to_hex
if __name__ == '__main__':
a, b = [gm.Position(x) for x in 'ab']
l = 2
a_in_w = gm.dot(gm.translation3(0, 0, 2), gm.translation3(0, 0, -a))
d_in_a = gm.rotation3_axis_angle(gm.vector3(0, 1, 0), gm.acos(a / l))
d_in_w = gm.dot(a_in_w, d_in_a)
A = gm.pos_of(a_in_w)
B = gm.dot(d_in_w, gm.point3(0, 0, l))
C = gm.dot(d_in_w, gm.point3(0, 0, l * 0.5))
D = gm.dot(d_in_w, gm.point3(0, 0, l * 0.25))
E = gm.dot(d_in_w, gm.point3(0, 0, l * 0.75))
lock_bound = gm.alg_not(
gm.alg_and(gm.less_than(b, 0.3), gm.less_than(1.99, a)))
# PLOT OF MOVEMENT
As = []
Bs = []
Cs = []
Ds = []
Es = []