Skip to content

Commit

Permalink
Update to np float 64
Browse files Browse the repository at this point in the history
  • Loading branch information
@mmatl
mmatl committed Jul 30, 2019
1 parent c39bb12 commit f53b926
Show file tree
Hide file tree
Showing 2 changed files with 27 additions and 27 deletions.
38 changes: 19 additions & 19 deletions urdfpy/urdf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -311,7 +311,7 @@ def size(self):

@size.setter
def size(self, value):
self._size = np.asanyarray(value).astype(np.float)
self._size = np.asanyarray(value).astype(np.float64)
self._meshes = []

@property
Expand Down Expand Up @@ -523,7 +523,7 @@ def scale(self):
@scale.setter
def scale(self, value):
if value is not None:
value = np.asanyarray(value).astype(np.float)
value = np.asanyarray(value).astype(np.float64)
self._scale = value

@property
Expand Down Expand Up @@ -891,7 +891,7 @@ def _from_xml(cls, node, path):
# Extract the color -- it's weirdly an attribute of a subelement
color = node.find('color')
if color is not None:
color = np.fromstring(color.attrib['rgba'], sep=' ')
color = np.fromstring(color.attrib['rgba'], sep=' ', dtype=np.float64)
kwargs['color'] = color

return Material(**kwargs)
Expand Down Expand Up @@ -1171,7 +1171,7 @@ def inertia(self):

@inertia.setter
def inertia(self, value):
value = np.asanyarray(value).astype(np.float)
value = np.asanyarray(value).astype(np.float64)
if not np.allclose(value, value.T):
raise ValueError('Inertia must be a symmetric matrix')
self._inertia = value
Expand Down Expand Up @@ -1201,7 +1201,7 @@ def _from_xml(cls, node, path):
[xx, xy, xz],
[xy, yy, yz],
[xz, yz, zz]
])
], dtype=np.float64)
return Inertial(mass=mass, inertia=inertia, origin=origin)

def _to_xml(self, parent, path):
Expand Down Expand Up @@ -2104,9 +2104,9 @@ def axis(self):
@axis.setter
def axis(self, value):
if value is None:
value = np.array([1.0, 0.0, 0.0])
value = np.array([1.0, 0.0, 0.0], dtype=np.float64)
else:
value = np.asanyarray(value).astype(np.float)
value = np.asanyarray(value, dtype=np.float64)
if value.shape != (3,):
raise ValueError('Invalid shape for axis, should be (3,)')
value = value / np.linalg.norm(value)
Expand Down Expand Up @@ -2258,24 +2258,24 @@ def get_child_pose(self, cfg=None):
cfg = 0.0
else:
cfg = float(cfg)
translation = np.eye(4)
translation = np.eye(4, dtype=np.float64)
translation[:3,3] = self.axis * cfg
return self.origin.dot(translation)
elif self.joint_type == 'planar':
if cfg is None:
cfg = np.zeros(2)
cfg = np.zeros(2, dtype=np.float64)
else:
cfg = np.asanyarray(cfg)
cfg = np.asanyarray(cfg, dtype=np.float64)
if cfg.shape != (2,):
raise ValueError(
'(2,) float configuration required for planar joints'
)
translation = np.eye(4)
translation = np.eye(4, dtype=np.float64)
translation[:3,3] = self.origin[:3,:2].dot(cfg)
return self.origin.dot(translation)
elif self.joint_type == 'floating':
if cfg is None:
cfg = np.zeros(6)
cfg = np.zeros(6, dtype=np.float64)
else:
cfg = configure_origin(cfg)
if cfg is None:
Expand Down Expand Up @@ -2927,7 +2927,7 @@ def link_fk(self, cfg=None, link=None, links=None, use_names=False):
for lnk in self._reverse_topo:
if lnk not in link_set:
continue
pose = np.eye(4)
pose = np.eye(4, dtype=np.float64)
path = self._paths_to_base[lnk]
for i in range(len(path) - 1):
child = path[i]
Expand Down Expand Up @@ -3012,7 +3012,7 @@ def link_fk_batch(self, cfgs=None, link=None, links=None, use_names=False):
for lnk in self._reverse_topo:
if lnk not in link_set:
continue
poses = np.tile(np.eye(4), (n_cfgs, 1, 1))
poses = np.tile(np.eye(4, dtype=np.float64), (n_cfgs, 1, 1))
path = self._paths_to_base[lnk]
for i in range(len(path) - 1):
child = path[i]
Expand Down Expand Up @@ -3137,7 +3137,7 @@ def visual_trimesh_fk(self, cfg=None, links=None):
pose = lfk[link].dot(visual.origin)
if visual.geometry.mesh is not None:
if visual.geometry.mesh.scale is not None:
S = np.eye(4)
S = np.eye(4, dtype=np.float64)
S[:3,:3] = np.diag(visual.geometry.mesh.scale)
pose = pose.dot(S)
fk[mesh] = pose
Expand Down Expand Up @@ -3175,7 +3175,7 @@ def visual_trimesh_fk_batch(self, cfgs=None, links=None):
poses = np.matmul(lfk[link], visual.origin)
if visual.geometry.mesh is not None:
if visual.geometry.mesh.scale is not None:
S = np.eye(4)
S = np.eye(4, dtype=np.float64)
S[:3,:3] = np.diag(visual.geometry.mesh.scale)
poses = np.matmul(poses, S)
fk[mesh] = poses
Expand Down Expand Up @@ -3372,14 +3372,14 @@ def animate(self, cfg_trajectory=None, loop_time=3.0, use_collision=False):
elif isinstance(ct, dict):
if len(ct) > 0:
for k in ct:
val = np.asanyarray(ct[k]).astype(np.float)
val = np.asanyarray(ct[k]).astype(np.float64)
if traj_len is None:
traj_len = len(val)
elif traj_len != len(val):
raise ValueError('Trajectories must be same length')
ct_np[k] = val
elif isinstance(ct, (list, tuple, np.ndarray)):
ct = np.asanyarray(ct)
ct = np.asanyarray(ct).astype(np.float64)
if ct.ndim == 1:
ct = ct.reshape(-1, 1)
if ct.ndim != 2 or ct.shape[1] != len(self.actuated_joints):
Expand Down Expand Up @@ -3816,7 +3816,7 @@ def _process_cfgs(self, cfgs):
elif cfgs[0] is None:
pass
else:
cfgs = np.asanyarray(cfgs)
cfgs = np.asanyarray(cfgs, dtype=np.float64)
for i, j in enumerate(self.actuated_joints):
joint_cfg[j] = cfgs[:,i]
else:
Expand Down
16 changes: 8 additions & 8 deletions urdfpy/utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,15 +28,15 @@ def rpy_to_matrix(coords):
R : (3,3) float
The corresponding homogenous 3x3 rotation matrix.
"""
coords = np.asanyarray(coords)
coords = np.asanyarray(coords, dtype=np.float64)
c3, c2, c1 = np.cos(coords)
s3, s2, s1 = np.sin(coords)

return np.array([
[c1 * c2, (c1 * s2 * s3) - (c3 * s1), (s1 * s3) + (c1 * c3 * s2)],
[c2 * s1, (c1 * c3) + (s1 * s2 * s3), (c3 * s1 * s2) - (c1 * s3)],
[-s2, c2 * s3, c2 * c3]
])
], dtype=np.float64)


def matrix_to_rpy(R, solution=1):
Expand Down Expand Up @@ -66,7 +66,7 @@ def matrix_to_rpy(R, solution=1):
coords : (3,) float
The roll-pitch-yaw coordinates in order (x-rot, y-rot, z-rot).
"""
R = np.asanyarray(R)
R = np.asanyarray(R, dtype=np.float64)
r = 0.0
p = 0.0
y = 0.0
Expand All @@ -87,7 +87,7 @@ def matrix_to_rpy(R, solution=1):
r = np.arctan2(R[2,1] / np.cos(p), R[2,2] / np.cos(p))
y = np.arctan2(R[1,0] / np.cos(p), R[0,0] / np.cos(p))

return np.array([r, p, y])
return np.array([r, p, y], dtype=np.float64)


def matrix_to_xyz_rpy(matrix):
Expand Down Expand Up @@ -121,7 +121,7 @@ def xyz_rpy_to_matrix(xyz_rpy):
matrix : (4,4) float
The homogenous transform matrix.
"""
matrix = np.eye(4)
matrix = np.eye(4, dtype=np.float64)
matrix[:3,3] = xyz_rpy[:3]
matrix[:3,:3] = rpy_to_matrix(xyz_rpy[3:])
return matrix
Expand All @@ -144,7 +144,7 @@ def parse_origin(node):
``origin`` child. Defaults to the identity matrix if no ``origin``
child was found.
"""
matrix = np.eye(4)
matrix = np.eye(4, dtype=np.float64)
origin_node = node.find('origin')
if origin_node is not None:
if 'xyz' in origin_node.attrib:
Expand Down Expand Up @@ -259,9 +259,9 @@ def configure_origin(value):
The created matrix.
"""
if value is None:
value = np.eye(4)
value = np.eye(4, dtype=np.float64)
elif isinstance(value, (list, tuple, np.ndarray)):
value = np.asanyarray(value).astype(np.float)
value = np.asanyarray(value, dtype=np.float64)
if value.shape == (6,):
value = xyz_rpy_to_matrix(value)
elif value.shape != (4,4):
Expand Down

0 comments on commit f53b926

Please sign in to comment.