def spin(self): # load and init ik solver for right hand velma_ikr = velmautils.VelmaIkSolver() velma_ikr.initIkSolver() # create Openrave interface self.openrave = openraveinstance.OpenraveInstance( PyKDL.Frame(PyKDL.Vector(0, 0, 0.1))) self.openrave.startNewThread() self.waitForOpenraveInit() print "openrave initialised" # create the robot interface for simulation velma = VelmaSim(self.openrave, velma_ikr) normals = velmautils.generateNormalsSphere(60.0 / 180.0 * math.pi) frames = velmautils.generateFramesForNormals(60.0 / 180.0 * math.pi, normals) distance = 0.1 space = [] max_solutions = 0 for x in np.arange(0.0, 1.01, distance): for y in np.arange(-1.0, 1.01, distance): for z in np.arange(1.0, 2.01, distance): solutions = 0 for fr in frames: T_Br_E = PyKDL.Frame(PyKDL.Vector(x, y, z)) * fr if self.openrave.findIkSolution(T_Br_E) != None: solutions += 1 space.append([x, y, z, solutions]) if solutions > max_solutions: max_solutions = solutions print "x: %s" % (x) print "points: %s" % (len(space)) m_id = 0 for s in space: value = float(s[3]) / float(max_solutions) m_id = publishSinglePointMarker( PyKDL.Vector(s[0], s[1], s[2]), m_id, r=1, g=value, b=value, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(distance * value, distance * value, distance * value), T=None) if m_id % 100 == 0: rospy.sleep(0.1) print "done" return
def spin(self): # load and init ik solver for right hand velma_ikr = velmautils.VelmaIkSolver() velma_ikr.initIkSolver() # create Openrave interface self.openrave = openraveinstance.OpenraveInstance(PyKDL.Frame(PyKDL.Vector(0,0,0.1))) self.openrave.startNewThread() self.waitForOpenraveInit() print "openrave initialised" # create the robot interface for simulation velma = VelmaSim(self.openrave, velma_ikr) normals = velmautils.generateNormalsSphere(60.0/180.0*math.pi) frames = velmautils.generateFramesForNormals(60.0/180.0*math.pi, normals) distance = 0.1 space = [] max_solutions = 0 for x in np.arange(0.0, 1.01, distance): for y in np.arange(-1.0, 1.01, distance): for z in np.arange(1.0, 2.01, distance): solutions = 0 for fr in frames: T_Br_E = PyKDL.Frame(PyKDL.Vector(x,y,z)) * fr if self.openrave.findIkSolution(T_Br_E) != None: solutions += 1 space.append([x,y,z,solutions]) if solutions > max_solutions: max_solutions = solutions print "x: %s"%(x) print "points: %s"%(len(space)) m_id = 0 for s in space: value = float(s[3]) / float(max_solutions) m_id = publishSinglePointMarker(PyKDL.Vector(s[0], s[1], s[2]), m_id, r=1, g=value, b=value, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(distance*value, distance*value, distance*value), T=None) if m_id%100 == 0: rospy.sleep(0.1) print "done" return
def spin(self): if True: # Create a world object world = ode.World() # world.setGravity( (0,0,-3.81) ) world.setGravity( (0,0,0) ) CFM = world.getCFM() ERP = world.getERP() print "CFM: %s ERP: %s"%(CFM, ERP) # world.setCFM(0.001) # print "CFM: %s ERP: %s"%(CFM, ERP) m_id = 0 self.pub_marker.eraseMarkers(0,3000, frame_id='world') # # Init Openrave # parser = OptionParser(description='Openrave Velma interface') OpenRAVEGlobalArguments.addOptions(parser) (options, leftargs) = parser.parse_args() self.env = OpenRAVEGlobalArguments.parseAndCreate(options,defaultviewer=True) # self.openrave_robot = self.env.ReadRobotXMLFile('robots/barretthand_ros.robot.xml') mimic_joints = [ ("right_HandFingerTwoKnuckleOneJoint", "right_HandFingerOneKnuckleOneJoint*1.0", "|right_HandFingerOneKnuckleOneJoint 1.0", ""), ("right_HandFingerOneKnuckleThreeJoint", "right_HandFingerOneKnuckleTwoJoint*0.33333", "|right_HandFingerOneKnuckleTwoJoint 0.33333", ""), ("right_HandFingerTwoKnuckleThreeJoint", "right_HandFingerTwoKnuckleTwoJoint*0.33333", "|right_HandFingerTwoKnuckleTwoJoint 0.33333", ""), ("right_HandFingerThreeKnuckleThreeJoint", "right_HandFingerThreeKnuckleTwoJoint*0.33333", "|right_HandFingerThreeKnuckleTwoJoint 0.33333", ""), ] rospack = rospkg.RosPack() self.urdf_module = RaveCreateModule(self.env, 'urdf') xacro_uri = rospack.get_path('barrett_hand_defs') + '/robots/barrett_hand.urdf.xml' urdf_uri = '/tmp/barrett_hand.urdf' srdf_uri = rospack.get_path('barrett_hand_defs') + '/robots/barrett_hand.srdf' arg1 = "collision_model_full:=true" arg2 = "collision_model_simplified:=false" arg3 = "collision_model_enlargement:=0.0" arg4 = "collision_model_no_hands:=false" subprocess.call(["rosrun", "xacro", "xacro", "-o", urdf_uri, xacro_uri, arg1, arg2, arg3, arg4]) robot_name = self.urdf_module.SendCommand('load ' + urdf_uri + ' ' + srdf_uri ) print "robot name: " + robot_name self.openrave_robot = self.env.GetRobot(robot_name) self.env.Remove(self.openrave_robot) for mimic in mimic_joints: mj = self.openrave_robot.GetJoint(mimic[0]) mj.SetMimicEquations(0, mimic[1], mimic[2], mimic[3]) self.openrave_robot.GetActiveManipulator().SetChuckingDirection([0,0,0,0])#directions) self.env.Add(self.openrave_robot) self.openrave_robot = self.env.GetRobot(robot_name) # print "robots: " # print self.env.GetRobots() joint_names = [] print "active joints:" for j in self.openrave_robot.GetJoints(): joint_names.append(j.GetName()) print j print "passive joints:" for j in self.openrave_robot.GetPassiveJoints(): joint_names.append(j.GetName()) print j # ODE does not support distance measure self.env.GetCollisionChecker().SetCollisionOptions(CollisionOptions.Contacts) self.env.Add(self.openrave_robot) vertices, faces = surfaceutils.readStl(rospack.get_path('velma_scripts') + "/data/meshes/klucz_gerda_ascii.stl", scale=1.0) # vertices, faces = surfaceutils.readStl("klucz_gerda_ascii.stl", scale=1.0) self.addTrimesh("object", vertices, faces) # self.addBox("object", 0.2,0.06,0.06) # self.addSphere("object", 0.15) # vertices, faces = self.getMesh("object") # # definition of the expected external wrenches # ext_wrenches = [] # origin of the external wrench (the end point of the key) wr_orig = PyKDL.Vector(0.039, 0.0, 0.0) for i in range(8): # expected force at the end point force = PyKDL.Frame(PyKDL.Rotation.RotX(float(i)/8.0 * 2.0 * math.pi)) * PyKDL.Vector(0,1,0) # expected torque at the com torque = wr_orig * force ext_wrenches.append(PyKDL.Wrench(force, torque)) # expected force at the end point force = PyKDL.Frame(PyKDL.Rotation.RotX(float(i)/8.0 * 2.0 * math.pi)) * PyKDL.Vector(-1,1,0) # expected torque at the com torque = wr_orig * force ext_wrenches.append(PyKDL.Wrench(force, torque)) # # definition of the grasps # grasp_id = 0 if grasp_id == 0: grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2 grasp_initial_configuration = [60.0/180.0*math.pi, None, None, None] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.122103662206, -0.124395758838, -0.702726011729, 0.689777190329), PyKDL.Vector(-0.00115787237883, -0.0194999426603, 0.458197712898)) # self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.174202588426, -0.177472708083, -0.691231954612, 0.678495061771), PyKDL.Vector(0.0, -0.0213436260819, 0.459123969078)) elif grasp_id == 1: grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, None, None, None] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0187387771868, -0.708157209758, -0.0317875569224, 0.705090018033), PyKDL.Vector(4.65661287308e-10, 0.00145332887769, 0.472836345434)) elif grasp_id == 2: grasp_direction = [0, 1, 0, 0] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, None, 90.0/180.0*math.pi, 0] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0187387763947, -0.708157179826, -0.0317875555789, 0.705089928626), PyKDL.Vector(0.0143095180392, 0.00145332887769, 0.483659058809)) elif grasp_id == 3: grasp_direction = [0, 1, 1, 1] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, None, None, None] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00518634245761, -0.706548316769, -0.0182458505507, 0.707410947861), PyKDL.Vector(0.000126354629174, -0.00217361748219, 0.47637796402)) elif grasp_id == 4: grasp_direction = [0, 0, 1, 0] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, 100.0/180.0*math.pi, None, 100.0/180.0*math.pi] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.153445252933, -0.161230275653, 0.681741576082, 0.696913201022), PyKDL.Vector(0.000126355327666, 0.00152841210365, 0.466048002243)) elif grasp_id == 5: grasp_direction = [0, 0, 1, 0] # spread, f1, f3, f2 grasp_initial_configuration = [100.0/180.0*math.pi, 101.5/180.0*math.pi, None, 101.5/180.0*math.pi] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.155488650062, -0.159260521271, 0.690572597636, 0.688163302213), PyKDL.Vector(-0.000278688268736, 0.00575117766857, 0.461560428143)) elif grasp_id == 6: grasp_direction = [0, 1, -1, 1] # spread, f1, f3, f2 grasp_initial_configuration = [90.0/180.0*math.pi, None, 0, 0] self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.512641041738, -0.485843507183, -0.514213889193, 0.48655882699), PyKDL.Vector(-0.000278423947748, -0.00292747467756, 0.445628076792)) else: print "ERROR: unknown grasp_id: %s"%(grasp_id) exit(0) self.updatePose("object", self.T_W_O) with open('barret_hand_openrave2ros_joint_map2.txt', 'r') as f: lines = f.readlines() joint_map = {} for line in lines: line_s = line.split() if len(line_s) == 2: joint_map[line_s[0]] = line_s[1] elif len(line_s) != 1: print "error in joint map file" exit(0) print joint_map self.pub_js = rospy.Publisher("/joint_states", JointState) if True: def getDirectionIndex(n): min_angle = -45.0/180.0*math.pi angle_range = 90.0/180.0*math.pi angles_count = 5 angles_count2 = angles_count * angles_count max_indices = angles_count2 * 6 if abs(n.x()) > abs(n.y()) and abs(n.x()) > abs(n.z()): if n.x() > 0: sec = 0 a1 = math.atan2(n.y(), n.x()) a2 = math.atan2(n.z(), n.x()) else: sec = 1 a1 = math.atan2(n.y(), -n.x()) a2 = math.atan2(n.z(), -n.x()) elif abs(n.y()) > abs(n.x()) and abs(n.y()) > abs(n.z()): if n.y() > 0: sec = 2 a1 = math.atan2(n.x(), n.y()) a2 = math.atan2(n.z(), n.y()) else: sec = 3 a1 = math.atan2(n.x(), -n.y()) a2 = math.atan2(n.z(), -n.y()) else: if n.z() > 0: sec = 4 a1 = math.atan2(n.x(), n.z()) a2 = math.atan2(n.y(), n.z()) else: sec = 5 a1 = math.atan2(n.x(), -n.z()) a2 = math.atan2(n.y(), -n.z()) a1i = int(angles_count*(a1-min_angle)/angle_range) a2i = int(angles_count*(a2-min_angle)/angle_range) if a1i < 0: print sec, a1i, a2i a1i = 0 if a1i >= angles_count: # print sec, a1i, a2i a1i = angles_count-1 if a2i < 0: print sec, a1i, a2i a2i = 0 if a2i >= angles_count: print sec, a1i, a2i a2i = angles_count-1 return sec * angles_count2 + a1i * angles_count + a2i # generate a dictionary of indexed directions normals_sphere_indexed_dir = velmautils.generateNormalsSphere(3.0/180.0*math.pi) print len(normals_sphere_indexed_dir) indices_directions = {} for n in normals_sphere_indexed_dir: index = getDirectionIndex(n) if not index in indices_directions: indices_directions[index] = [n] else: indices_directions[index].append(n) for index in indices_directions: n_mean = PyKDL.Vector() for n in indices_directions[index]: n_mean += n n_mean.Normalize() indices_directions[index] = n_mean # test direction indexing if False: normals_sphere = velmautils.generateNormalsSphere(3.0/180.0*math.pi) print len(normals_sphere) m_id = 0 for current_index in range(5*5*6): count = 0 r = random.random() g = random.random() b = random.random() for n in normals_sphere: sec = -1 a1 = None a2 = None index = getDirectionIndex(n) if index == current_index: m_id = self.pub_marker.publishSinglePointMarker(n*0.1, m_id, r=r, g=g, b=b, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.007, 0.007, 0.007), T=None) count += 1 print current_index, count for index in indices_directions: m_id = self.pub_marker.publishSinglePointMarker(indices_directions[index]*0.12, m_id, r=1, g=1, b=1, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.012, 0.012, 0.012), T=None) exit(0) # # PyODE test # if True: fixed_joints_names_for_fixed_DOF = [ ["right_HandFingerOneKnuckleOneJoint", "right_HandFingerTwoKnuckleOneJoint"], # spread ["right_HandFingerOneKnuckleTwoJoint", "right_HandFingerOneKnuckleThreeJoint"], # f1 ["right_HandFingerThreeKnuckleTwoJoint", "right_HandFingerThreeKnuckleThreeJoint"], # f3 ["right_HandFingerTwoKnuckleTwoJoint", "right_HandFingerTwoKnuckleThreeJoint"], # f2 ] coupled_joint_names_for_fingers = ["right_HandFingerOneKnuckleThreeJoint", "right_HandFingerTwoKnuckleThreeJoint", "right_HandFingerThreeKnuckleThreeJoint"] actuated_joints_for_DOF = [ ["right_HandFingerOneKnuckleOneJoint", "right_HandFingerTwoKnuckleOneJoint"], # spread ["right_HandFingerOneKnuckleTwoJoint"], # f1 ["right_HandFingerThreeKnuckleTwoJoint"], # f3 ["right_HandFingerTwoKnuckleTwoJoint"]] # f2 ignored_links = ["world", "world_link"] self.run_ode_simulation = False self.insert6DofGlobalMarker(self.T_W_O) print "grasp_direction = [%s, %s, %s, %s] # spread, f1, f3, f2"%(grasp_direction[0], grasp_direction[1], grasp_direction[2], grasp_direction[3]) print "grasp_initial_configuration = [%s, %s, %s, %s]"%(grasp_initial_configuration[0], grasp_initial_configuration[1], grasp_initial_configuration[2], grasp_initial_configuration[3]) print "grasp_final_configuration = %s"%(self.openrave_robot.GetDOFValues()) rot_q = self.T_W_O.M.GetQuaternion() print "self.T_W_O = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s, %s, %s, %s), PyKDL.Vector(%s, %s, %s))"%(rot_q[0], rot_q[1], rot_q[2], rot_q[3], self.T_W_O.p.x(), self.T_W_O.p.y(), self.T_W_O.p.z()) self.erase6DofMarker() raw_input("Press ENTER to continue...") # reset the gripper in Openrave self.openrave_robot.SetDOFValues([0,0,0,0]) # read the position of all links grasp_links_poses = {} for link in self.openrave_robot.GetLinks(): grasp_links_poses[link.GetName()] = self.OpenraveToKDL(link.GetTransform()) # print "obtained the gripper configuration for the grasp:" # print finalconfig[0] # # simulation in ODE # # Create a world object world = ode.World() # world.setGravity( (0,0,-3.81) ) world.setGravity( (0,0,0) ) CFM = world.getCFM() ERP = world.getERP() print "CFM: %s ERP: %s"%(CFM, ERP) # world.setCFM(0.001) # print "CFM: %s ERP: %s"%(CFM, ERP) self.space = ode.Space() # Create a body inside the world body = ode.Body(world) M = ode.Mass() M.setCylinderTotal(0.02, 1, 0.005, 0.09) body.setMass(M) ode_mesh = ode.TriMeshData() ode_mesh.build(vertices, faces) geom = ode.GeomTriMesh(ode_mesh, self.space) geom.name = "object" geom.setBody(body) self.setOdeBodyPose(geom, self.T_W_O) ode_gripper_geoms = {} for link in self.openrave_robot.GetLinks(): link_name = link.GetName() if link_name in ignored_links: print "ignoring: %s"%(link_name) continue print "adding: %s"%(link_name) ode_mesh_link = None body_link = None T_W_L = self.OpenraveToKDL(link.GetTransform()) col = link.GetCollisionData() vertices = col.vertices faces = col.indices ode_mesh_link = ode.TriMeshData() ode_mesh_link.build(vertices, faces) ode_gripper_geoms[link_name] = ode.GeomTriMesh(ode_mesh_link, self.space) if True: body_link = ode.Body(world) M_link = ode.Mass() M_link.setCylinderTotal(0.05, 1, 0.01, 0.09) body_link.setMass(M_link) ode_gripper_geoms[link_name].setBody(body_link) ode_gripper_geoms[link_name].name = link.GetName() self.setOdeBodyPose(body_link, T_W_L) actuated_joint_names = [] for dof in range(4): for joint_name in actuated_joints_for_DOF[dof]: actuated_joint_names.append(joint_name) ode_gripper_joints = {} for joint_name in joint_names: joint = self.openrave_robot.GetJoint(joint_name) link_parent = joint.GetHierarchyParentLink().GetName() link_child = joint.GetHierarchyChildLink().GetName() if link_parent in ode_gripper_geoms and link_child in ode_gripper_geoms: axis = joint.GetAxis() anchor = joint.GetAnchor() if joint_name in actuated_joint_names: ode_gripper_joints[joint_name] = ode.AMotor(world) ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody()) ode_gripper_joints[joint_name].setMode(ode.AMotorUser)#Euler) # ode_gripper_joints[joint_name].setNumAxes(3) ode_gripper_joints[joint_name].setAxis(0,1,axis) # ode_gripper_joints[joint_name].setAxis(1,1,axis) # ode_gripper_joints[joint_name].setAxis(2,1,axis) ode_gripper_joints[joint_name].setParam(ode.ParamFMax, 10.0) # ode_gripper_joints[joint_name].setParam(ode.ParamFMax2, 10.0) # ode_gripper_joints[joint_name].setParam(ode.ParamFMax3, 10.0) else: ode_gripper_joints[joint_name] = ode.HingeJoint(world) ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody()) ode_gripper_joints[joint_name].setAxis(-axis) ode_gripper_joints[joint_name].setAnchor(anchor) # ode_gripper_joints[joint_name] = ode.HingeJoint(world) # ode_gripper_joints[joint_name].attach(ode_gripper_geoms[link_parent].getBody(), ode_gripper_geoms[link_child].getBody()) # ode_gripper_joints[joint_name].setAxis(-axis) # ode_gripper_joints[joint_name].setAnchor(anchor) # if joint_name in actuated_joint_names: # ode_gripper_joints[joint_name].setParam(ode.ParamFMax, 10.0) limits = joint.GetLimits() value = joint.GetValue(0) # print joint_name # print "limits: %s %s"%(limits[0], limits[1]) # print "axis: %s"%(axis) # print "anchor: %s"%(anchor) # print "value: %s"%(value) lim = [limits[0], limits[1]] if limits[0] <= -math.pi: # print "lower joint limit %s <= -PI, setting to -PI+0.01"%(limits[0]) lim[0] = -math.pi + 0.01 if limits[1] >= math.pi: # print "upper joint limit %s >= PI, setting to PI-0.01"%(limits[1]) lim[1] = math.pi - 0.01 ode_gripper_joints[joint_name].setParam(ode.ParamLoStop, lim[0]) ode_gripper_joints[joint_name].setParam(ode.ParamHiStop, lim[1]) # ode_gripper_joints[joint_name].setParam(ode.ParamFudgeFactor, 0.5) ode_fixed_joint = ode.FixedJoint(world) ode_fixed_joint.attach(None, ode_gripper_geoms["right_HandPalmLink"].getBody()) ode_fixed_joint.setFixed() # # set the poses of all links as for the grasp # for link_name in grasp_links_poses: if link_name in ignored_links: continue ode_body = ode_gripper_geoms[link_name].getBody() T_W_L = grasp_links_poses[link_name] self.setOdeBodyPose(ode_body, T_W_L) fixed_joint_names = [] fixed_joint_names += coupled_joint_names_for_fingers for dof in range(4): if grasp_direction[dof] == 0.0: for joint_name in fixed_joints_names_for_fixed_DOF[dof]: if not joint_name in fixed_joint_names: fixed_joint_names.append(joint_name) # # change all coupled joints to fixed joints # fixed_joints = {} for joint_name in fixed_joint_names: # save the bodies attached body0 = ode_gripper_joints[joint_name].getBody(0) body1 = ode_gripper_joints[joint_name].getBody(1) # save the joint angle if joint_name in actuated_joint_names: angle = ode_gripper_joints[joint_name].getAngle(0) else: angle = ode_gripper_joints[joint_name].getAngle() # detach the joint ode_gripper_joints[joint_name].attach(None, None) del ode_gripper_joints[joint_name] fixed_joints[joint_name] = [ode.FixedJoint(world), angle] fixed_joints[joint_name][0].attach(body0, body1) fixed_joints[joint_name][0].setFixed() # change all actuated joints to motor joints # actuated_joint_names = [] # for dof in range(4): # for joint_name in actuated_joints_for_DOF[dof]: # actuated_joint_names.append(joint_name) # for joint_name in actuated_joint_names: # A joint group for the contact joints that are generated whenever # two bodies collide contactgroup = ode.JointGroup() print "ode_gripper_geoms:" print ode_gripper_geoms initial_T_W_O = self.T_W_O # Do the simulation... dt = 0.001 total_time = 0.0 failure = False # # PID # Kc = 1.0 # Ti = 1000000000000.0 KcTi = 0.0 Td = 0.0 Ts = 0.001 pos_d = 1.0 e0 = 0.0 e1 = 0.0 e2 = 0.0 u = 0.0 u_max = 1.0 while total_time < 5.0 and not rospy.is_shutdown(): # # ODE simulation # joint = ode_gripper_joints["right_HandFingerOneKnuckleTwoJoint"] e2 = e1 e1 = e0 e0 = pos_d - joint.getAngle(0) u = u + Kc * (e0 - e1) + KcTi * Ts * e0 + Kc * Td / Ts * (e0 - 2.0 * e1 + e2) if u > u_max: u = u_max if u < -u_max: u = -u_max joint.setParam(ode.ParamFMax, 10000.0) joint.setParam(ode.ParamVel, 1.1) # joint.setParam(ode.ParamVel2, 1.1) # joint.setParam(ode.ParamVel3, 1.1) print joint.getAngle(0), " ", u #, " ", joint.getAngleRate(0) # print u # joint.addTorque(u) # for dof in range(4): # for joint_name in actuated_joints_for_DOF[dof]: # if joint_name in ode_gripper_joints: # ode_gripper_joints[joint_name].addTorque(1*grasp_direction[dof]) self.grasp_contacts = [] self.space.collide((world,contactgroup), self.near_callback) world.step(dt) total_time += dt contactgroup.empty() # # ROS interface # old_m_id = m_id m_id = 0 # publish frames from ODE if False: for link_name in ode_gripper_geoms: link_body = ode_gripper_geoms[link_name].getBody() if link_body == None: link_body = ode_gripper_geoms[link_name] T_W_Lsim = self.getOdeBodyPose(link_body) m_id = self.pub_marker.publishFrameMarker(T_W_Lsim, m_id, scale=0.05, frame='world', namespace='default') # publish the mesh of the object T_W_Osim = self.getOdeBodyPose(body) m_id = self.pub_marker.publishConstantMeshMarker("package://velma_scripts/data/meshes/klucz_gerda_binary.stl", m_id, r=1, g=0, b=0, scale=1.0, frame_id='world', namespace='default', T=T_W_Osim) # update the gripper visualization in ros js = JointState() js.header.stamp = rospy.Time.now() for jn in joint_map: ros_joint_name = joint_map[jn] js.name.append(ros_joint_name) if jn in ode_gripper_joints: if jn in actuated_joint_names: js.position.append( ode_gripper_joints[jn].getAngle(0) ) else: js.position.append( ode_gripper_joints[jn].getAngle() ) elif jn in fixed_joints: js.position.append(fixed_joints[jn][1]) else: js.position.append(0) self.pub_js.publish(js) # draw contacts for c in self.grasp_contacts: cc = PyKDL.Vector(c[0], c[1], c[2]) cn = PyKDL.Vector(c[3], c[4], c[5]) m_id = self.pub_marker.publishVectorMarker(cc, cc+cn*0.04, m_id, 1, 0, 0, frame='world', namespace='default', scale=0.003) if m_id < old_m_id: self.pub_marker.eraseMarkers(m_id,old_m_id+1, frame_id='world') diff_T_W_O = PyKDL.diff(initial_T_W_O, T_W_Osim) # if diff_T_W_O.vel.Norm() > 0.02 or diff_T_W_O.rot.Norm() > 30.0/180.0*math.pi: # print "the object moved" # print diff_T_W_O # failure = True # break rospy.sleep(0.01) if not failure: T_O_Wsim = T_W_Osim.Inverse() TR_O_Wsim = PyKDL.Frame(T_O_Wsim.M) contacts = [] for c in self.grasp_contacts: cc_W = PyKDL.Vector(c[0], c[1], c[2]) cn_W = PyKDL.Vector(c[3], c[4], c[5]) cc_O = T_O_Wsim * cc_W cn_O = TR_O_Wsim * cn_W contacts.append([cc_O[0], cc_O[1], cc_O[2], cn_O[0], cn_O[1], cn_O[2]]) gws, contact_planes = self.generateGWS(contacts, 1.0) grasp_quality = None for wr in ext_wrenches: wr_qual = self.getQualityMeasure2(gws, wr) if grasp_quality == None or wr_qual < grasp_quality: grasp_quality = wr_qual grasp_quality_classic = self.getQualityMeasure(gws) print "grasp_quality_classic: %s grasp_quality: %s"%(grasp_quality_classic, grasp_quality) exit(0)
def __init__(self, vol_radius, vol_samples_count, T_H_O, orientations_angle, vertices_obj, faces_obj): self.vol_radius = vol_radius self.vol_samples_count = vol_samples_count self.index_factor = float(self.vol_samples_count)/(2.0*self.vol_radius) self.vol_samples = [] for x in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count): self.vol_samples.append([]) for y in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count): self.vol_samples[-1].append([]) for z in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count): self.vol_samples[-1][-1].append([]) self.vol_samples[-1][-1][-1] = {} self.vol_sample_points = [] for xi in range(self.vol_samples_count): for yi in range(self.vol_samples_count): for zi in range(self.vol_samples_count): self.vol_sample_points.append( self.getVolPoint(xi,yi,zi) ) self.T_H_O = T_H_O self.T_O_H = self.T_H_O.Inverse() # generate the set of orientations self.orientations_angle = orientations_angle normals_sphere = velmautils.generateNormalsSphere(self.orientations_angle) orientations1 = velmautils.generateFramesForNormals(self.orientations_angle, normals_sphere) orientations2 = [] for ori in orientations1: x_axis = ori * PyKDL.Vector(1,0,0) if x_axis.z() > 0.0: orientations2.append(ori) self.orientations = {} for ori_idx in range(len(orientations2)): self.orientations[ori_idx] = orientations2[ori_idx] # generate a set of surface points self.surface_points_obj = surfaceutils.sampleMeshDetailedRays(vertices_obj, faces_obj, 0.0015) print "surface of the object has %s points"%(len(self.surface_points_obj)) # disallow contact with the surface points beyond the key handle for p in self.surface_points_obj: if p.pos.x() > 0.0: p.allowed = False surface_points_obj_init = [] surface_points2_obj_init = [] for sp in self.surface_points_obj: if sp.allowed: surface_points_obj_init.append(sp) else: surface_points2_obj_init.append(sp) print "generating a subset of surface points of the object..." while True: p_idx = random.randint(0, len(self.surface_points_obj)-1) if self.surface_points_obj[p_idx].allowed: break p_dist = 0.003 self.sampled_points_obj = [] while True: self.sampled_points_obj.append(p_idx) surface_points_obj2 = [] for sp in surface_points_obj_init: if (sp.pos-self.surface_points_obj[p_idx].pos).Norm() > p_dist: surface_points_obj2.append(sp) if len(surface_points_obj2) == 0: break surface_points_obj_init = surface_points_obj2 p_idx = surface_points_obj_init[0].id print "subset size: %s"%(len(self.sampled_points_obj)) print "generating a subset of other surface points of the object..." p_dist2 = 0.006 while True: p_idx = random.randint(0, len(self.surface_points_obj)-1) if not self.surface_points_obj[p_idx].allowed: break self.sampled_points2_obj = [] while True: self.sampled_points2_obj.append(p_idx) surface_points2_obj2 = [] for sp in surface_points2_obj_init: if (sp.pos-self.surface_points_obj[p_idx].pos).Norm() > p_dist2: surface_points2_obj2.append(sp) if len(surface_points2_obj2) == 0: break surface_points2_obj_init = surface_points2_obj2 p_idx = surface_points2_obj_init[0].id # test volumetric model if False: for pt_idx in self.sampled_points2_obj: pt = self.surface_points_obj[pt_idx] m_id = self.pub_marker.publishSinglePointMarker(pt.pos, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.003, 0.003, 0.003), T=None) rospy.sleep(0.001) for pt_idx in self.sampled_points_obj: pt = self.surface_points_obj[pt_idx] m_id = self.pub_marker.publishSinglePointMarker(pt.pos, m_id, r=0, g=1, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.003, 0.003, 0.003), T=None) rospy.sleep(0.001) print "subset size: %s"%(len(self.sampled_points2_obj)) print "calculating surface curvature at sampled points of the obj..." m_id = 0 planes = 0 edges = 0 points = 0 for pt_idx in range(len(self.surface_points_obj)): indices, nx, pc1, pc2 = surfaceutils.pclPrincipalCurvaturesEstimation(self.surface_points_obj, pt_idx, 5, 0.003) # m_id = self.pub_marker.publishVectorMarker(self.T_W_O * self.surface_points_obj[pt_idx].pos, self.T_W_O * (self.surface_points_obj[pt_idx].pos + nx*0.004), m_id, 1, 0, 0, frame='world', namespace='default', scale=0.0002) # m_id = self.pub_marker.publishVectorMarker(self.T_W_O * self.surface_points_obj[pt_idx].pos, self.T_W_O * (self.surface_points_obj[pt_idx].pos + self.surface_points_obj[pt_idx].normal*0.004), m_id, 0, 0, 1, frame='world', namespace='default', scale=0.0002) self.surface_points_obj[pt_idx].frame = PyKDL.Frame(PyKDL.Rotation(nx, self.surface_points_obj[pt_idx].normal * nx, self.surface_points_obj[pt_idx].normal), self.surface_points_obj[pt_idx].pos) self.surface_points_obj[pt_idx].pc1 = pc1 self.surface_points_obj[pt_idx].pc2 = pc2 if pc1 < 0.2: self.surface_points_obj[pt_idx].is_plane = True self.surface_points_obj[pt_idx].is_edge = False self.surface_points_obj[pt_idx].is_point = False planes += 1 elif pc2 < 0.2: self.surface_points_obj[pt_idx].is_plane = False self.surface_points_obj[pt_idx].is_edge = True self.surface_points_obj[pt_idx].is_point = False edges += 1 else: self.surface_points_obj[pt_idx].is_plane = False self.surface_points_obj[pt_idx].is_edge = False self.surface_points_obj[pt_idx].is_point = True points += 1 print "obj planes: %s edges: %s points: %s"%(planes, edges, points)