def spherical_linear_interpolation_fromR(R0, Rf, h):
# See https://en.wikipedia.org/wiki/Slerp
q0 = np.array(so3.quaternion(R0))
qf = np.array(so3.quaternion(Rf))
v0 = np.array(MathUtils.normalize(q0))
vf = np.array(MathUtils.normalize(qf))
dot = np.dot(v0, vf)
if dot < 0.0:
vf = -vf
dot = -dot
# TODO: Is this causing non rotation matrixes?
dot_threshold = 0.995
if dot > dot_threshold:
# print(f"WARNING: dot={dot} > threshold={dot_threshold}\t linearly interprolating")
# If results are close, calculate new result linearly
result = v0 + h * (vf - v0)
result_as_list = result.tolist()
q = MathUtils.normalize(result_as_list)
else:
theta_0 = np.arccos(dot)
theta = theta_0 * h
sin_theta = np.sin(theta)
sin_theta_0 = np.sin(theta_0)
s0 = np.cos(theta) - dot * sin_theta / sin_theta_0
s1 = sin_theta / sin_theta_0
q = (s0 * v0) + (s1 * vf)
return so3.from_quaternion(q)
def split_db(self):
filenames = []
precision = int(np.ceil(np.log10(len(self.db.keys()))))
n = 0
for k in sorted(self.db.keys()):
fname = os.path.splitext(
self.filename)[0] + '_' + str(n).zfill(precision)
filenames.append(fname)
f = open(fname + '.csv', 'w')
for pose in self.db[k]:
f.write(','.join([str(d) for d in k]))
f.write(',')
values = []
pose = se3.from_homogeneous(pose)
# saving pose.t
values += pose[1]
# saving pose.q
q = np.array(so3.quaternion(pose[0]))
values += list(q[1:4])
values.append(q[0])
f.write(','.join([str(v) for v in values]))
f.write('\n')
f.close()
n = n + 1
return filenames
def launch_test_mvbb_grasps(robotname, box_db, links_to_check = None):
"""Launches a very simple program that spawns a box with dimensions specified in boxes_db.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname, world)
xform = resource.get("default_initial_%s.xform" % robotname, description="Initial hand transform",
default=se3.identity(), world=world, doedit=False)
for box_dims, poses in box_db.db.items():
if world.numRigidObjects() > 0:
world.remove(world.rigidObject(0))
obj = make_box(world,
box_dims[0],
box_dims[1],
box_dims[2])
poses_filtered = []
R,t = obj.getTransform()
obj.setTransform(R, [0, 0, box_dims[2]/2.])
w_T_o = np.array(se3.homogeneous(obj.getTransform()))
p_T_h = np.array(se3.homogeneous(xform))
p_T_h[2][3] += 0.02
for pose in poses:
w_T_p = w_T_o.dot(pose)
w_T_h_des_se3 = se3.from_homogeneous(w_T_p.dot(p_T_h))
if CollisionTestPose(world, robot, obj, w_T_h_des_se3):
pose_pp = se3.from_homogeneous(pose)
t_pp = pose_pp[1]
q_pp = so3.quaternion(pose_pp[0])
q_pp = [q_pp[1], q_pp[2], q_pp[3], q_pp[0]]
print "Pose", t_pp + q_pp, "has been filtered since it is in collision for box", box_dims
elif w_T_p[2][3] <= 0.:
print "Pose", t_pp + q_pp, "has been filtered since it penetrates with table"
else:
poses_filtered.append(pose)
print "Filtered", len(poses)-len(poses_filtered), "out of", len(poses), "poses"
# embed()
# create a hand emulator from the given robot name
module = importlib.import_module('plugins.' + robotname)
# emulator takes the robot index (0), start link index (6), and start driver index (6)
program = FilteredMVBBTesterVisualizer(box_dims,
poses_filtered,
world,
p_T_h,
module,
box_db,
links_to_check)
vis.setPlugin(None)
vis.setPlugin(program)
program.reshape(800, 600)
vis.show()
# this code manually updates the visualization
while vis.shown():
time.sleep(0.1)
return
def save_simulation(self, box_dims, pose, h_T_o, q, c_p, c_f):
if len(c_p) != self.n_l * 3:
raise RuntimeError(
'Incorrect size of c_p when calling save_simulation()')
if len(c_f) != self.n_l * 6:
raise RuntimeError(
'Incorrect size of c_f when calling save_simulation()')
if len(q) != self.n_dofs:
raise RuntimeError(
'Incorrect size of q when calling save_simulation()')
if not self.has_simulation(box_dims, pose):
f = open(self.filename_simulated, 'a')
values = list(box_dims)
if isinstance(pose, np.ndarray):
pose = se3.from_homogeneous(pose)
# saving pose.t
values += pose[1]
# saving pose.q
q_wxyz = np.array(so3.quaternion(pose[0]))
values += list(q_wxyz[1:4])
values.append(q_wxyz[0])
if isinstance(h_T_o, np.ndarray):
h_T_o = se3.from_homogeneous(h_T_o)
# saving h_T_o.t
values += h_T_o[1]
# saving h_T_o.q
q_wxyz = np.array(so3.quaternion(h_T_o[0]))
values += list(q_wxyz[1:4])
values.append(q_wxyz[0])
# saving q
values += list(q)
values += list(c_p)
values += list(c_f)
f.write(','.join([str(v) for v in values]))
f.write('\n')
f.close()
self._load_mvbbs_simulated()
def publish_object(world_, msg):
# A simple function for publishing object frame, pose and twist
# Getting the transform from world to object and object velocity
obj = world_.rigidObject(world_.numRigidObjects() - 1)
(R_obj, t_obj) = obj.getTransform()
(w_obj, v_obj) = obj.getVelocity()
quat_obj = so3.quaternion(R_obj)
# Setting the transform message and broadcasting object frame
msg.header.stamp = rospy.Time.now()
msg.child_frame_id = object_frame_name
msg.transform.translation.x = t_obj[0]
msg.transform.translation.y = t_obj[1]
msg.transform.translation.z = t_obj[2]
msg.transform.rotation.x = quat_obj[1]
msg.transform.rotation.y = quat_obj[2]
msg.transform.rotation.z = quat_obj[3]
msg.transform.rotation.w = quat_obj[0]
global_vars.obj_broadcaster.sendTransform(msg)
# Publishing the object pose and twist
obj_pose = Pose()
obj_pose.position.x = msg.transform.translation.x
obj_pose.position.y = msg.transform.translation.y
obj_pose.position.z = msg.transform.translation.z
obj_pose.orientation.x = msg.transform.rotation.x
obj_pose.orientation.y = msg.transform.rotation.y
obj_pose.orientation.z = msg.transform.rotation.z
global_vars.obj_pose_pub.publish(obj_pose)
obj_twist = Twist()
# Publishing null twist now
# obj_twist.angular.x = w_obj[0]
# obj_twist.angular.y = w_obj[1]
# obj_twist.angular.z = w_obj[2]
# obj_twist.linear.x = v_obj[0]
# obj_twist.linear.y = v_obj[1]
# obj_twist.linear.z = v_obj[2]
obj_twist.angular.x = 0.0
obj_twist.angular.y = 0.0
obj_twist.angular.z = 0.0
obj_twist.linear.x = 0.0
obj_twist.linear.y = 0.0
obj_twist.linear.z = 0.0
global_vars.obj_twist_pub.publish(obj_twist)
def publish_palm(robot, msg):
# Simple function for publishing the palm frame
# Getting the transform from world to floating frame
floating_link = robot.link(
5
) # This id comes from trial and error (should be kuka coupler bottom)
(R, t) = floating_link.getTransform()
quat = so3.quaternion(R)
# Setting the transform message and broadcasting palm frame
msg.header.stamp = rospy.Time.now()
msg.child_frame_id = floating_frame_name
msg.transform.translation.x = t[0]
msg.transform.translation.y = t[1]
msg.transform.translation.z = t[2]
msg.transform.rotation.x = quat[1]
msg.transform.rotation.y = quat[2]
msg.transform.rotation.z = quat[3]
msg.transform.rotation.w = quat[0]
global_vars.palm_broadcaster.sendTransform(msg)
def transform_average(Ts):
t = vectorops.div(vectorops.add(*[T[1] for T in Ts]), len(Ts))
qs = [so3.quaternion(T[0]) for T in Ts]
q = vectorops.div(vectorops.add(*qs), len(Ts))
return (so3.from_quaternion(q), t)
def to_Quaternion(klampt_so3):
"""From Klamp't so3 element to ROS Quaternion"""
q = so3.quaternion(klampt_so3)
ros_q = Quaternion()
ros_q.w, ros_q.x, ros_q.y, ros_q.z = q
return ros_q
def broadcast_tf(broadcaster,
klampt_obj,
frameprefix="klampt",
root="world",
stamp=0.):
"""Broadcasts Klampt frames to the ROS tf module.
Args:
broadcaster (tf.TransformBroadcaster): the tf broadcaster
klampt_obj: the object to publish. Can be WorldModel, Simulator,
RobotModel, SimRobotController, anything with a getTransform
or getCurrentTransform method, or se3 element
frameprefix (str): the name of the base frame for this object
root (str): the name of the TF world frame.
Note:
If klampt_obj is a Simulator or SimRobotController, then its
corresponding model will be updated.
"""
from klampt import WorldModel, Simulator, RobotModel, SimRobotController
if stamp == 'now':
stamp = rospy.Time.now()
elif isinstance(stamp, (int, float)):
stamp = rospy.Time(stamp)
world = None
if isinstance(klampt_obj, WorldModel):
world = klampt_obj
elif isinstance(klampt_obj, Simulator):
world = klampt_obj.model()
klampt_obj.updateModel()
if world is not None:
prefix = frameprefix
for i in range(world.numRigidObjects()):
T = world.rigidObject(i).getTransform()
q = so3.quaternion(T[0])
broadcaster.sendTransform(
T[1], (q[1], q[2], q[3], q[0]), stamp, root,
prefix + "/" + world.rigidObject(i).getName())
for i in range(world.numRobots()):
robot = world.robot(i)
rprefix = prefix + "/" + robot.getName()
broadcast_tf(broadcaster, robot, rprefix, root)
return
robot = None
if isinstance(klampt_obj, RobotModel):
robot = klampt_obj
elif isinstance(klampt_obj, SimRobotController):
robot = klampt_obj.model()
robot.setConfig(klampt_obj.getSensedConfig())
if robot is not None:
rprefix = frameprefix
for j in range(robot.numLinks()):
p = robot.link(j).getParent()
if p < 0:
T = robot.link(j).getTransform()
q = so3.quaternion(T[0])
broadcaster.sendTransform(
T[1], (q[1], q[2], q[3], q[0]), stamp, root,
rprefix + "/" + robot.link(j).getName())
else:
Tparent = se3.mul(se3.inv(robot.link(p).getTransform()),
robot.link(j).getTransform())
q = so3.quaternion(Tparent[0])
broadcaster.sendTransform(
Tparent[1], (q[1], q[2], q[3], q[0]), stamp,
rprefix + "/" + robot.link(p).getName(),
rprefix + "/" + robot.link(j).getName())
return
transform = None
if isinstance(klampt_obj, (list, tuple)):
#assume to be an SE3 element.
transform = klampt_obj
elif hasattr(klampt_obj, 'getTransform'):
transform = klampt_obj.getTransform()
elif hasattr(klampt_obj, 'getCurrentTransform'):
transform = klampt_obj.getCurrentTransform()
else:
raise ValueError("Invalid type given to broadcast_tf: ",
klampt_obj.__class__.__name__)
q = so3.quaternion(transform[0])
broadcaster.sendTransform(transform[1], (q[1], q[2], q[3], q[0]), stamp,
root, frameprefix)
return
def normalize_rotation(R):
"""Given a matrix close to a proper (orthogonal) rotation matrix,
returns a true orthogonal matrix."""
q = so3.quaternion(R) #normalizes
return so3.from_quaternion(q)