def create_box(env, name, transform, dims, color=[0, 0, 1]): infobox = OpenRAVEBody.create_body_info(dims, color, 0, True) box = RaveCreateKinBody(env, '') box.InitFromGeometries([infobox]) box.SetName(name) box.SetTransform(transform) return box
def create_cylinder(env, body_name, t, dims, color=[0, 1, 1]): infocylinder = OpenRAVEBody.create_body_info(GeometryType.Cylinder, dims, color) if type(env) != Environment: import ipdb ipdb.set_trace() cylinder = RaveCreateKinBody(env, '') cylinder.InitFromGeometries([infocylinder]) cylinder.SetName(body_name) cylinder.SetTransform(t) return cylinder
class OpenRAVEBody(object): def __init__(self, env, name, geom): assert env is not None self.name = name self._env = env self._geom = geom if isinstance(geom, Circle): self._add_circle(geom) elif isinstance(geom, Can): self._add_can(geom) elif isinstance(geom, Obstacle): self._add_obstacle(geom) elif isinstance(geom, PR2): self._add_robot(geom) elif isinstance(geom, Table): self._add_table(geom) elif isinstance(geom, Wall): self._add_wall(geom) elif isinstance(geom, Box): self._add_box(geom) else: raise OpenRAVEException( "Geometry not supported for %s for OpenRAVEBody" % geom) def delete(self): self._env.Remove(self.env_body) def set_transparency(self, transparency): for link in self.env_body.GetLinks(): for geom in link.GetGeometries(): geom.SetTransparency(transparency) def _add_circle(self, geom): color = [1, 0, 0] if hasattr(geom, "color") and geom.color == 'blue': color = [0, 0, 1] elif hasattr(geom, "color") and geom.color == 'green': color = [0, 1, 0] elif hasattr(geom, "color") and geom.color == 'red': color = [1, 0, 0] self.env_body = OpenRAVEBody.create_cylinder(self._env, self.name, np.eye(4), [geom.radius, 2], color) self._env.AddKinBody(self.env_body) def _add_can(self, geom): color = [1, 0, 0] if hasattr(geom, "color") and geom.color == 'blue': color = [0, 0, 1] elif hasattr(geom, "color") and geom.color == 'green': color = [0, 1, 0] elif hasattr(geom, "color") and geom.color == 'red': color = [1, 0, 0] self.env_body = OpenRAVEBody.create_cylinder( self._env, self.name, np.eye(4), [geom.radius, geom.height], color) self._env.AddKinBody(self.env_body) def _add_obstacle(self, geom): obstacles = np.matrix('-0.576036866359447, 0.918128654970760, 1;\ -0.806451612903226,-1.07017543859649, 1;\ 1.01843317972350,-0.988304093567252, 1;\ 0.640552995391705,0.906432748538011, 1;\ -0.576036866359447, 0.918128654970760, -1;\ -0.806451612903226,-1.07017543859649, -1;\ 1.01843317972350,-0.988304093567252, -1;\ 0.640552995391705,0.906432748538011, -1') body = RaveCreateKinBody(self._env, '') vertices = np.array(obstacles) indices = np.array([[0, 1, 2], [2, 3, 0], [4, 5, 6], [6, 7, 4], [0, 4, 5], [0, 1, 5], [1, 2, 5], [5, 6, 2], [2, 3, 6], [6, 7, 3], [0, 3, 7], [0, 4, 7]]) body.InitFromTrimesh(trimesh=TriMesh(vertices, indices), draw=True) body.SetName(self.name) for link in body.GetLinks(): for geom in link.GetGeometries(): geom.SetDiffuseColor((.9, .9, .9)) self.env_body = body self._env.AddKinBody(body) def _add_box(self, geom): infobox = OpenRAVEBody.create_body_info(KinBody.Link.GeomType.Box, geom.dim, [0.5, 0.2, 0.1]) self.env_body = RaveCreateKinBody(self._env, '') self.env_body.InitFromGeometries([infobox]) self.env_body.SetName(self.name) self._env.Add(self.env_body) def _add_wall(self, geom): self.env_body = OpenRAVEBody.create_wall(self._env, geom.wall_type) self.env_body.SetName(self.name) self._env.Add(self.env_body) def _add_robot(self, geom): self.env_body = self._env.ReadRobotXMLFile(geom.shape) self.env_body.SetName(self.name) self._env.Add(self.env_body) def _add_table(self, geom): self.env_body = OpenRAVEBody.create_table(self._env, geom) self.env_body.SetName(self.name) self._env.Add(self.env_body) def set_pose(self, base_pose, rotation=None): trans = None if isinstance(self._geom, Circle) or isinstance( self._geom, Obstacle) or isinstance(self._geom, Wall): trans = OpenRAVEBody.base_pose_2D_to_mat(base_pose) elif isinstance(self._geom, PR2): trans = OpenRAVEBody.base_pose_to_mat(base_pose) elif isinstance(self._geom, Table) or isinstance( self._geom, Can) or isinstance(self._geom, Box): assert rotation != None trans = OpenRAVEBody.transform_from_obj_pose(base_pose, rotation) self.env_body.SetTransform(trans) def set_dof(self, back_height, l_arm_pose, l_gripper, r_arm_pose, r_gripper): """ This function assumed to be called when the self.env_body is a robot and its geom is type PR2 It sets the DOF values for important joint of PR2 back_height: back_height attribute of type Value l_arm_pose: l_arm_pose attribute of type Vector7d l_gripper: l_gripper attribute of type Value r_arm_pose: r_arm_pose attribute of type Vector7d r_gripper: r_gripper attribute of type Value """ # Get current dof value for each joint dof_val = self.env_body.GetActiveDOFValues() # Obtain indices of left arm and right arm l_arm_inds = self.env_body.GetManipulator('leftarm').GetArmIndices() l_gripper_ind = self.env_body.GetJoint( 'l_gripper_l_finger_joint').GetDOFIndex() r_arm_inds = self.env_body.GetManipulator('rightarm').GetArmIndices() r_gripper_ind = self.env_body.GetJoint( 'r_gripper_l_finger_joint').GetDOFIndex() b_height_ind = self.env_body.GetJoint('torso_lift_joint').GetDOFIndex() # Update the DOF value dof_val[b_height_ind] = back_height dof_val[l_arm_inds], dof_val[l_gripper_ind] = l_arm_pose, l_gripper dof_val[r_arm_inds], dof_val[r_gripper_ind] = r_arm_pose, r_gripper # Set new DOF value to the robot self.env_body.SetActiveDOFValues(dof_val) def _set_active_dof_inds(self, inds=None): """ Set active dof index to the one we are interested This function is implemented to simplify jacobian calculation in the CollisionPredicate inds: Optional list of index specifying dof index we are interested in """ robot = self.env_body if inds == None: dof_inds = np.ndarray(0, dtype=np.int) dof_inds = np.r_[dof_inds, robot.GetJoint("torso_lift_joint").GetDOFIndex()] dof_inds = np.r_[dof_inds, robot.GetManipulator("leftarm").GetArmIndices()] dof_inds = np.r_[ dof_inds, robot.GetManipulator("leftarm").GetGripperIndices()] dof_inds = np.r_[dof_inds, robot.GetManipulator("rightarm").GetArmIndices()] dof_inds = np.r_[ dof_inds, robot.GetManipulator("rightarm").GetGripperIndices()] robot.SetActiveDOFs( dof_inds, DOFAffine.X + DOFAffine.Y + DOFAffine.RotationAxis, [0, 0, 1]) else: robot.SetActiveDOFs(inds) @staticmethod def create_cylinder(env, body_name, t, dims, color=[0, 1, 1]): infocylinder = OpenRAVEBody.create_body_info(GeometryType.Cylinder, dims, color) if type(env) != Environment: import ipdb ipdb.set_trace() cylinder = RaveCreateKinBody(env, '') cylinder.InitFromGeometries([infocylinder]) cylinder.SetName(body_name) cylinder.SetTransform(t) return cylinder @staticmethod def create_box(env, name, transform, dims, color=[0, 0, 1]): infobox = OpenRAVEBody.create_body_info(dims, color, 0, True) box = RaveCreateKinBody(env, '') box.InitFromGeometries([infobox]) box.SetName(name) box.SetTransform(transform) return box @staticmethod def create_body_info(body_type, dims, color, transparency=0.8, visible=True): infobox = KinBody.Link.GeometryInfo() infobox._type = body_type infobox._vGeomData = dims infobox._bVisible = True infobox._fTransparency = transparency infobox._vDiffuseColor = color return infobox @staticmethod def create_wall(env, wall_type): component_type = KinBody.Link.GeomType.Box wall_color = [0.5, 0.2, 0.1] box_infos = [] if wall_type == 'closet': wall_endpoints = [[-1.0, -3.0], [-1.0, 4.0], [1.9, 4.0], [1.9, 8.0], [5.0, 8.0], [5.0, 4.0], [8.0, 4.0], [8.0, -3.0], [-1.0, -3.0]] else: raise NotImplemented for i, (start, end) in enumerate(zip(wall_endpoints[0:-1], wall_endpoints[1:])): dim_x, dim_y = 0, 0 thickness = WALL_THICKNESS if start[0] == end[0]: ind_same, ind_diff = 0, 1 length = abs(start[ind_diff] - end[ind_diff]) dim_x, dim_y = thickness, length / 2 + thickness elif start[1] == end[1]: ind_same, ind_diff = 1, 0 length = abs(start[ind_diff] - end[ind_diff]) dim_x, dim_y = length / 2 + thickness, thickness else: raise NotImplemented, 'Can only create axis-aligned walls' transform = np.eye(4) transform[ind_same, 3] = start[ind_same] if start[ind_diff] < end[ind_diff]: transform[ind_diff, 3] = start[ind_diff] + length / 2 else: transform[ind_diff, 3] = end[ind_diff] + length / 2 dims = [dim_x, dim_y, 1] box_info = OpenRAVEBody.create_body_info(component_type, dims, wall_color) box_info._t = transform box_infos.append(box_info) wall = RaveCreateRobot(env, '') wall.InitFromGeometries(box_infos) return wall @staticmethod def create_table(env, geom): thickness = geom.thickness leg_height = geom.leg_height back = geom.back dim1, dim2 = geom.table_dim legdim1, legdim2 = geom.leg_dim table_color = [0.5, 0.2, 0.1] component_type = KinBody.Link.GeomType.Box tabletop = OpenRAVEBody.create_body_info( component_type, [dim1 / 2, dim2 / 2, thickness / 2], table_color) leg1 = OpenRAVEBody.create_body_info( component_type, [legdim1 / 2, legdim2 / 2, leg_height / 2], table_color) leg1._t[0, 3] = dim1 / 2 - legdim1 / 2 leg1._t[1, 3] = dim2 / 2 - legdim2 / 2 leg1._t[2, 3] = -leg_height / 2 - thickness / 2 leg2 = OpenRAVEBody.create_body_info( component_type, [legdim1 / 2, legdim2 / 2, leg_height / 2], table_color) leg2._t[0, 3] = dim1 / 2 - legdim1 / 2 leg2._t[1, 3] = -dim2 / 2 + legdim2 / 2 leg2._t[2, 3] = -leg_height / 2 - thickness / 2 leg3 = OpenRAVEBody.create_body_info( component_type, [legdim1 / 2, legdim2 / 2, leg_height / 2], table_color) leg3._t[0, 3] = -dim1 / 2 + legdim1 / 2 leg3._t[1, 3] = dim2 / 2 - legdim2 / 2 leg3._t[2, 3] = -leg_height / 2 - thickness / 2 leg4 = OpenRAVEBody.create_body_info( component_type, [legdim1 / 2, legdim2 / 2, leg_height / 2], table_color) leg4._t[0, 3] = -dim1 / 2 + legdim1 / 2 leg4._t[1, 3] = -dim2 / 2 + legdim2 / 2 leg4._t[2, 3] = -leg_height / 2 - thickness / 2 if back: back_plate = OpenRAVEBody.create_body_info( component_type, [legdim1 / 10, dim2 / 2, leg_height - thickness / 2], table_color) back_plate._t[0, 3] = dim1 / 2 - legdim1 / 10 back_plate._t[1, 3] = 0 back_plate._t[2, 3] = -leg_height / 2 - thickness / 4 table = RaveCreateRobot(env, '') if not back: table.InitFromGeometries([tabletop, leg1, leg2, leg3, leg4]) else: table.InitFromGeometries( [tabletop, leg1, leg2, leg3, leg4, back_plate]) return table @staticmethod def base_pose_2D_to_mat(pose): # x, y = pose assert len(pose) == 2 x = pose[0] y = pose[1] rot = 0 q = quatFromAxisAngle((0, 0, rot)).tolist() pos = [x, y, 0] # pos = np.vstack((x,y,np.zeros(1))) matrix = matrixFromPose(q + pos) return matrix @staticmethod def base_pose_3D_to_mat(pose): # x, y, z = pose assert len(pose) == 3 x = pose[0] y = pose[1] z = pose[2] rot = 0 q = quatFromAxisAngle((0, 0, rot)).tolist() pos = [x, y, z] # pos = np.vstack((x,y,np.zeros(1))) matrix = matrixFromPose(q + pos) return matrix @staticmethod def mat_to_base_pose_2D(mat): pose = poseFromMatrix(mat) x = pose[4] y = pose[5] return np.array([x, y]) @staticmethod def base_pose_to_mat(pose): # x, y, rot = pose assert len(pose) == 3 x = pose[0] y = pose[1] rot = pose[2] q = quatFromAxisAngle((0, 0, rot)).tolist() pos = [x, y, 0] # pos = np.vstack((x,y,np.zeros(1))) matrix = matrixFromPose(q + pos) return matrix @staticmethod def mat_to_base_pose(mat): pose = poseFromMatrix(mat) x = pose[4] y = pose[5] rot = axisAngleFromRotationMatrix(mat)[2] return np.array([x, y, rot]) @staticmethod def obj_pose_from_transform(transform): trans = transform[:3, 3] rot_matrix = transform[:3, :3] yaw, pitch, roll = OpenRAVEBody._ypr_from_rot_matrix(rot_matrix) # ipdb.set_trace() return np.array((trans[0], trans[1], trans[2], yaw, pitch, roll)) @staticmethod def transform_from_obj_pose(pose, rotation=np.array([0, 0, 0])): x, y, z = pose alpha, beta, gamma = rotation Rz, Ry, Rx = OpenRAVEBody._axis_rot_matrices(pose, rotation) rot_mat = np.dot(Rz, np.dot(Ry, Rx)) matrix = np.eye(4) matrix[:3, :3] = rot_mat matrix[:3, 3] = [x, y, z] return matrix @staticmethod def _axis_rot_matrices(pose, rotation): x, y, z = pose alpha, beta, gamma = rotation Rz_2d = np.array([[cos(alpha), -sin(alpha)], [sin(alpha), cos(alpha)]]) Ry_2d = np.array([[cos(beta), sin(beta)], [-sin(beta), cos(beta)]]) Rx_2d = np.array([[cos(gamma), -sin(gamma)], [sin(gamma), cos(gamma)]]) I = np.eye(3) Rz = I.copy() Rz[:2, :2] = Rz_2d Ry = I.copy() Ry[[[0], [2]], [0, 2]] = Ry_2d Rx = I.copy() Rx[1:3, 1:3] = Rx_2d # ipdb.set_trace() return Rz, Ry, Rx @staticmethod def _ypr_from_rot_matrix(r): # alpha yaw = atan2(r[1, 0], r[0, 0]) # beta pitch = atan2(-r[2, 0], np.sqrt(r[2, 1]**2 + r[2, 2]**2)) # gamma roll = atan2(r[2, 1], r[2, 2]) # ipdb.set_trace() return (yaw, pitch, roll) def ik_arm_pose(self, ee_pos, ee_rot): assert isinstance(self._geom, PR2) manip = self.env_body.GetManipulator('rightarm_torso') iktype = IkParameterizationType.Transform6D solution = self.ik_solution(manip, iktype, ee_pos, ee_rot) return solution def ik_solution(self, manip, iktype, ee_pos, ee_rot): ee_trans = OpenRAVEBody.transform_from_obj_pose(ee_pos, ee_rot) # Openravepy flip the rotation axis by 90 degree, thus we need to change it back rot_mat = matrixFromAxisAngle([0, np.pi / 2, 0]) ee_trans = ee_trans.dot(rot_mat) solutions = manip.FindIKSolutions(IkParameterization(ee_trans, iktype), IkFilterOptions.CheckEnvCollisions) return solutions