def from_numpy(obj, type='auto', template=None):
"""Converts a numpy array or multiple numpy arrays to a Klamp't object.
Supports:
* lists and tuples
* RigidTransform: accepts a 4x4 homogeneous coordinate transform
* Matrix3, Rotation: accepts a 3x3 matrix.
* Configs
* Trajectory: accepts a pair (times,milestones)
* TriangleMesh: accepts a pair (verts,indices)
* PointCloud: accepts a n x (3+k) array, where k is the # of properties
* VolumeGrid: accepts a triple (bmin,bmax,array)
* Geometry3D: accepts a pair (T,geomdata)
"""
global supportedTypes
if type == 'auto' and template is not None:
otype = types.objectToTypes(template)
if isinstance(otype, (list, tuple)):
for t in otype:
if t in supportedTypes:
type = t
break
if type == 'auto':
raise ValueError('obj is not a supported type: ' +
', '.join(otype))
else:
type = otype
if type == 'auto':
if isinstance(obj, (tuple, list)):
if all(isinstance(v, np.ndarray) for v in obj):
if len(obj) == 2:
if len(obj[0].shape) == 1 and len(obj[1].shape) == 2:
type = 'Trajectory'
elif len(obj[0].shape) == 2 and len(
obj[1].shape
) == 2 and obj[0].shape[1] == 3 and obj[1].shape[1] == 3:
type = 'TriangleMesh'
if len(obj) == 3:
if obj[0].shape == (3, ) and obj[1].shape == (3, ):
type = 'VolumeGrid'
if type == 'auto':
raise ValueError(
"Can't auto-detect type of list of shapes" +
', '.join(str(v.shape) for v in obj))
else:
if isinstance(obj[0], np.ndarray) and obj[0].shape == (4, 4):
type = 'Geometry3D'
else:
raise ValueError(
"Can't auto-detect type of irregular list")
else:
assert isinstance(
obj, np.ndarray
), "Can only convert lists, tuples, and arrays from numpy"
if obj.shape == (3, 3):
type = 'Matrix3'
elif obj.shape == (4, 4):
type = 'RigidTransform'
elif len(obj.shape) == 1:
type = 'Config'
else:
raise ValueError("Can't auto-detect type of matrix of shape " +
str(obj.shape))
if type not in supportedTypes:
raise ValueError(type + ' is not a supported type')
if type == 'RigidTransform':
return se3.from_homogeneous(obj)
elif type == 'Rotation' or type == 'Matrix3':
return so3.from_matrix(obj)
elif type == 'Trajectory':
assert len(obj) == 2, "Trajectory format is (times,milestones)"
times = obj[0].tolist()
milestones = obj[1].tolist()
if template is not None:
return template.constructor()(times, milestones)
from klampt.model.trajectory import Trajectory
return Trajectory(times, milestones)
elif type == 'TriangleMesh':
from klampt import TriangleMesh
res = TriangleMesh()
vflat = obj[0].flatten()
res.vertices.resize(len(vflat))
for i, v in enumerate(vflat):
res.vertices[i] = float(v)
iflat = obj[1].flatten()
res.indices.resize(len(iflat))
for i, v in enumerate(iflat):
res.indices[i] = int(v)
return res
elif type == 'PointCloud':
from klampt import PointCloud
assert len(obj.shape) == 2, "PointCloud array must be a 2D array"
assert obj.shape[1] >= 3, "PointCloud array must have at least 3 values"
points = obj[:, :3]
properties = obj[:, 3:]
res = PointCloud()
res.setPoints(points.shape[0], points.flatten())
if template is not None:
if len(template.propertyNames) != properties.shape[1]:
raise ValueError(
"Template object doesn't have the same properties as the numpy object"
)
for i in range(len(template.propertyNames)):
res.propertyNames[i] = template.propertyNames[i]
else:
for i in range(properties.shape[1]):
res.propertyNames.append('property %d' % (i + 1))
if len(res.propertyNames) > 0:
res.properties.resize(len(res.propertyNames) * points.shape[0])
if obj.shape[1] >= 3:
res.setProperties(properties.flatten())
return res
elif type == 'VolumeGrid':
from klampt import VolumeGrid
assert len(obj) == 3, "VolumeGrid format is (bmin,bmax,values)"
assert len(obj[2].shape) == 3, "VolumeGrid values must be a 3D array"
bmin = obj[0]
bmax = obj[1]
values = obj[2]
res = VolumeGrid()
res.bbox.append(bmin[0])
res.bbox.append(bmin[1])
res.bbox.append(bmin[2])
res.bbox.append(bmax[0])
res.bbox.append(bmax[1])
res.bbox.append(bmax[2])
res.dims.append(values.shape[0])
res.dims.append(values.shape[1])
res.dims.append(values.shape[2])
vflat = values.flatten()
res.values.resize(len(vflat))
for i, v in enumerate(vflat):
res.values[i] = v
return res
elif type == 'Group':
from klampt import Geometry3D
res = Geometry3D()
assert isinstance(
obj,
(list, tuple)), "Group format is a list or tuple of Geometry3D's"
for i in range(len(obj)):
res.setElement(i, from_numpy(obj[i], 'Geometry3D'))
return res
elif type == 'Geometry3D':
from klampt import Geometry3D
if not isinstance(obj, (list, tuple)) or len(obj) != 2:
raise ValueError("Geometry3D must be a (transform,geometry) tuple")
T = from_numpy(obj[0], 'RigidTransform')
geomdata = obj[1]
subtype = None
if template is not None:
subtype = template.type()
if subtype == 'PointCloud':
g = Geometry3D(
from_numpy(geomdata, subtype, template.getPointCloud()))
else:
g = Geometry3D(from_numpy(geomdata, subtype))
g.setCurrentTransform(*T)
return g
subtype = 'Group'
if all(isinstance(v, np.ndarray) for v in geomdata):
if len(geomdata) == 2:
if len(geomdata[0].shape) == 1 and len(geomdata[1].shape) == 2:
subtype = 'Trajectory'
elif len(geomdata[0].shape) == 2 and len(
geomdata[1].shape) == 2 and geomdata[0].shape[
1] == 3 and geomdata[1].shape[1] == 3:
subtype = 'TriangleMesh'
if len(geomdata) == 3:
if geomdata[0].shape == (3, ) and geomdata[1].shape == (3, ):
subtype = 'VolumeGrid'
g = Geometry3D(from_numpy(obj, subtype))
g.setCurrentTransform(*T)
return g
else:
return obj.flatten()
def calculation():
"""
this is the main function of calibration calculation color.
"""
joint_positions = []
data_file = open(calidata_path + 'calibration_joint_positions.txt', 'r')
for line in data_file:
line = line.rstrip()
l = [num for num in line.split(' ')]
l2 = [float(num) for num in l]
joint_positions.append(l2)
data_file.close()
#the marker transforms here are actually just points not transformations
marker_transforms = []
data_file = open(calidata_path + 'calibration_marker_transforms.txt', 'r')
for line in data_file:
line = line.rstrip()
l = [num for num in line.split(' ')]
l2 = [float(num) for num in l]
marker_transforms.append(l2)
data_file.close()
T_marker_assumed = loader.load(
'RigidTransform', calidata_path + 'assumed_marker_world_transform.txt')
T_marker_assumed = (so3.inv(T_marker_assumed[0]), T_marker_assumed[1])
world = WorldModel()
res = world.readFile("robot_model_data/ur5Blocks.xml")
if not res:
raise RuntimeError("unable to load model")
#vis.add("world",world)
robot = world.robot(0)
link = robot.link(7)
q0 = robot.getConfig()
Tcameras = []
Tlinks = []
for i in range(len(joint_positions)):
q = robot.getConfig()
q[1:7] = joint_positions[i][0:6]
robot.setConfig(q)
Tlinks.append(link.getTransform())
#vis.add("ghost"+str(i),q)
#vis.setColor("ghost"+str(i),0,1,0,0.5)
robot.setConfig(q0)
#vis.add("MARKER",se3.mul(link.getTransform(),T_marker_assumed))
#vis.setAttribute("MARKER","width",2)
for loop in range(NLOOPS if ESTIMATE_MARKER_TRANSFORM else 1):
###Calculate Tcamera in EE (only needed for transform fit..)
###Tcameras = [se3.mul(se3.inv(Tl),se3.mul(T_marker_assumed,se3.inv(Tm_c))) for (Tl,Tm_c) in zip(Tlinks,marker_transforms)]
#actually using only point fit here....
if METHOD == 'point fit':
Tcamera2 = point_fit_transform(
[Tm_c for Tm_c in marker_transforms],
[se3.mul(se3.inv(Tl), T_marker_assumed)[1] for Tl in Tlinks])
Tcamera2 = [so3.from_matrix(Tcamera2[0]), Tcamera2[1]]
Tcamera = Tcamera2
#else:
# Tcamera = transform_average(Tcameras)
#with the Tcamera from the current iteration, calculate marker points in world
Tmarkers_world = [
se3.apply(se3.mul(Tl, Tcamera), Tm_c)
for (Tl, Tm_c) in zip(Tlinks, marker_transforms)
]
#Tmarkers_link = [se3.mul(se3.inv(Tl),Tm) for Tl,Tm in zip(Tlinks,Tmarkers)]
if ESTIMATE_MARKER_TRANSFORM:
T_marker_assumed_inv = point_fit_transform(
[[0, 0, 0] for Tm_c in marker_transforms], [
se3.apply(se3.mul(Tl, Tcamera2), Tm_c)
for (Tl, Tm_c) in zip(Tlinks, marker_transforms)
])
T_marker_assumed_inv = [
so3.from_matrix(T_marker_assumed_inv[0]),
T_marker_assumed_inv[1]
]
#print T_marker_assumed_inv
T_marker_assumed = T_marker_assumed_inv
#print "New assumed marker position",T_marker_assumed
else:
pass
#print "Would want new marker transform",transform_average(Tmarkers_world)
#print "New estimated camera position",Tcamera
SSE_t = 0
for (Tm_c, Tl) in zip(marker_transforms, Tlinks):
Tm_c_est = se3.mul(se3.mul(se3.inv(Tcamera), se3.inv(Tl)),
T_marker_assumed)
SSE_t += vectorops.distanceSquared(Tm_c, Tm_c_est[1])
#print "RMSE rotation (radians)",np.sqrt(SSE_R/len(Tlinks))
print "RMSE translations (meters)", np.sqrt(SSE_t / len(Tlinks))
#Tcameras = [se3.mul(se3.inv(Tl),se3.mul(T_marker_assumed,se3.inv(Tm_c))) for (Tl,Tm_c) in zip(Tlinks,marker_transforms)]
#Tmarkers = [se3.mul(Tl,se3.mul(Tcamera,Tm_c)) for (Tl,Tm_c) in zip(Tlinks,marker_transforms)]
print "Saving to calibrated_camera_xform.txt"
cali_txt_path = calidata_path + "calibrated_camera_xform.txt"
loader.save(Tcamera, "RigidTransform", cali_txt_path)
if ESTIMATE_MARKER_TRANSFORM:
print "Saving to calibrated_marker_link_xform.txt"
loader.save(T_marker_assumed, "RigidTransform",
calidata_path + "calibrated_marker_world_xform.txt")
#SSE_R = 0
SSE_t = 0
for (Tm_c, Tl) in zip(marker_transforms, Tlinks):
Tm_c_est = se3.mul(se3.mul(se3.inv(Tcamera), se3.inv(Tl)),
T_marker_assumed)
SSE_t += vectorops.distanceSquared(Tm_c, Tm_c_est[1])
print "RMSE translations (meters)", np.sqrt(SSE_t / len(Tlinks))
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, SimRobotSensor
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 xrange(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,
prefix + "/" + world.rigidObject(i).getName(), root)
for i in xrange(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 xrange(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,
rprefix + "/" + robot.link(j).getName(), root)
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(j).getName(),
rprefix + "/" + robot.link(p).getName())
return
if isinstance(klampt_obj, SimRobotSensor):
from ..model import sensing
Tsensor_link = sensing.get_sensor_xform(klampt_obj)
link = int(klampt_obj.getSetting('link'))
if klampt_obj.type(
) == 'LaserRangeSensor': #the convention between Klampt and ROS is different
klampt_to_ros_lidar = so3.from_matrix([[0, 1, 0], [0, 0, 1],
[1, 0, 0]])
Tsensor_link = (so3.mul(Tsensor_link[0],
klampt_to_ros_lidar), Tsensor_link[1])
q = so3.quaternion(Tsensor_link[0])
robot = klampt_obj.robot()
broadcaster.sendTransform(
Tsensor_link[1], (q[1], q[2], q[3], q[0]), stamp,
frameprefix + "/" + robot.getName() + '/' + klampt_obj.name(),
frameprefix + "/" + robot.link(link).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,
frameprefix, root)
return