def getJacobian(self, base_name, end_name, q): if not (base_name, end_name) in self.fk_solvers: self.fk_solvers[(base_name, end_name)] = FkIkSolver.FkSolver( self.tree, base_name, end_name, self.joint_names_vector) fk_solver = self.fk_solvers[(base_name, end_name)] # extract joint values for the chain q_jac = PyKDL.JntArray(fk_solver.chain_length) q_jac_idx = 0 for q_idx in fk_solver.q_indices_map: q_jac[q_jac_idx] = q[q_idx] q_jac_idx += 1 jac_small = PyKDL.Jacobian(fk_solver.chain.getNrOfJoints()) fk_solver.jac_solver.JntToJac(q_jac, jac_small) # create the jacobian for all joints jac_big = np.matrix(np.zeros((6, len(q)))) for col_idx in range(jac_small.columns()): q_idx = fk_solver.q_indices_map[col_idx] col = jac_small.getColumn(col_idx) for row_idx in range(6): jac_big[row_idx, q_idx] = col[row_idx] return jac_big
def __init__(self, urdf, world='world', tip='tip'): # Try loading the URDF data into a KDL tree. (ok, self.tree) = kdlp.treeFromString(urdf) assert ok, "Unable to parse the URDF" # Save the base and tip frame names. self.baseframe = world self.tipframe = tip # Create the isolated chain from world to tip. self.chain = self.tree.getChain(world, tip) # Extract the number of joints. self.N = self.chain.getNrOfJoints() assert self.N > 0, "Found no joints in the chain" # Create storage for the joint position, tip position, and # Jacobian matrices (for the KDL library). self.qkdl = kdl.JntArray(self.N) self.Tkdl = kdl.Frame() self.Jkdl = kdl.Jacobian(self.N) # Also pre-allocate the memory for the numpy return variables. # The content will be overwritten so the initial values are # irrelevant. self.p = np.zeros((3, 1)) self.R = np.identity(3) self.J = np.zeros((6, self.N)) # Instantiate the solvers for tip position and Jacobian. self.fkinsolver = kdl.ChainFkSolverPos_recursive(self.chain) self.jacsolver = kdl.ChainJntToJacSolver(self.chain)
def jacobian(self, joint_values=None): """Compute the Jacobian for the current joint positions.""" if joint_values is None: joint_values = self.joint_angles jac = PyKDL.Jacobian(self.n_joints) self._jac_kdl.JntToJac(self.joints_to_kdl('positions', joint_values), jac) return self.kdl_to_mat(jac)
def jacobian(self, joint_values=None): """Compute the Jacobian for the current joint positions.""" jnt_array = self.joints_to_kdl('positions') jac = PyKDL.Jacobian(jnt_array.rows()) self._jac_kdl.JntToJac(jnt_array, jac) mat = self.kdl_to_mat(jac) return mat
def update_joints(self, joint_msg): for i, n in enumerate(self.joint_names): index = joint_msg.name.index(n) self.joints[i] = joint_msg.position[index] if self.debug: self.debug_count += 1 if (self.debug_count % 10) == 0: frame = kdl.Frame() self.fk_ee.JntToCart(self.joints, frame) print " " print " " print "frame translation:" print frame.p print "frame rotation:" print frame.M print " " jacobian = kdl.Jacobian(self.num_joints) self.jac_ee.JntToJac(self.joints, jacobian) print "jacobian" print jacobian
def get_jacobian(self, d_chain): # Obtain jacobian for the selected arm if d_chain == 'left_chain': jacobian = kdl.Jacobian(self.left_chain.getNrOfJoints()) self.jac_solver_left.JntToJac(self.left_jnt_pos, jacobian) # print '\n Left: \n', jacobian elif d_chain == 'right_chain': jacobian = kdl.Jacobian(self.right_chain.getNrOfJoints()) self.jac_solver_right.JntToJac(self.right_jnt_pos, jacobian) # print '\n Right \n', jacobian else: rospy.logerr('Wrong chain specified for Jacobian') jacobian = kdl.Jacobian(self.chain.getNrOfJoints()) return jacobian
def jacobian(self, joint_values): jacobian = PyKDL.Jacobian(self.NUM_JOINTS) joint_array = PyKDL.JntArray(self.NUM_JOINTS) for i, val in enumerate(joint_values): joint_array[i] = val self.jac_solver.JntToJac(joint_array, jacobian) return self.kdl_to_mat(jacobian)
def jacobian(self): ''' Calculates jacobian based on joint values - argument to pass ( to do ) ''' joint_values = self.phi jacobian = PyKDL.Jacobian(self.num_joints) self.jac_kdl.JntToJac(self.joints_to_kdl(joint_values), jacobian) #changes jacobian to self.jac_matrix temp = jacobian return self.kdl_to_mat(temp)
def jacobian(self, q, pos=None): j_kdl = kdl.Jacobian(self.num_joints) q_kdl = joint_list_to_kdl(q) self._jac_kdl.JntToJac(q_kdl, j_kdl) if pos is not None: ee_pos = self.forward(q)[:3, 3] pos_kdl = kdl.Vector(pos[0] - ee_pos[0], pos[1] - ee_pos[1], pos[2] - ee_pos[2]) j_kdl.changeRefPoint(pos_kdl) return kdl_to_mat(j_kdl)
def jacobian(self, joint_values=None, end_link=None): """Compute the Jacobian for the current joint positions.""" if end_link is not None: jnt_array = self.joints_to_kdl( 'positions', last_joint=self.joint_names[self.link_names.index(end_link)]) jac = PyKDL.Jacobian(jnt_array.rows()) jac_kdl_part = self._jac_kdl_part[end_link] jac_kdl_part.JntToJac(jnt_array, jac) mat = self.kdl_to_mat(jac) mat_add = np.matrix( np.zeros((mat.shape[0], mat.shape[0] - mat.shape[1]))) mat = np.concatenate((mat, mat_add), axis=1) else: jnt_array = self.joints_to_kdl('positions') jac = PyKDL.Jacobian(jnt_array.rows()) self._jac_kdl.JntToJac(jnt_array, jac) mat = self.kdl_to_mat(jac) return mat
def get_jacobian(self): # Obtain jacobian for the selected arm self.jac_solver = kdl.ChainJntToJacSolver(self.chain) jacobian = kdl.Jacobian(self.nJoints) self.jac_solver.JntToJac(self.joint_values_kdl, jacobian) jac_array = np.empty(0) for row in range(jacobian.rows()): for col in range(jacobian.columns()): jac_array = np.hstack((jac_array, jacobian[row, col])) jac_array = np.reshape(jac_array, (jacobian.rows(), jacobian.columns())) return jac_array
def get_jacobian(self): #initialize seed array from current joint states seed_array = PyKDL.JntArray(len(self._joint_positions)) for idx, val in enumerate(self._joint_positions): seed_array[idx] = val #compute the current jacobian and convert it into numpy array jacobian = PyKDL.Jacobian(len(self._joint_positions)) self._jac_kdl.JntToJac(seed_array, jacobian) J = np.zeros([int(jacobian.rows()), int(jacobian.columns())]) for i in range(int(jacobian.rows())): for j in range(int(jacobian.columns())): J[i, j] = jacobian[i, j] return J
def Jac_kdl(self,arm,q): ''' returns the Jacobian, given the joint angles ''' J_kdl = kdl.Jacobian(7) self.cody_kdl[arm]['jacobian_solver'].JntToJac(q,J_kdl) kdl_jac = np.matrix([ [J_kdl[0,0],J_kdl[0,1],J_kdl[0,2],J_kdl[0,3],J_kdl[0,4],J_kdl[0,5],J_kdl[0,6]], [J_kdl[1,0],J_kdl[1,1],J_kdl[1,2],J_kdl[1,3],J_kdl[1,4],J_kdl[1,5],J_kdl[1,6]], [J_kdl[2,0],J_kdl[2,1],J_kdl[2,2],J_kdl[2,3],J_kdl[2,4],J_kdl[2,5],J_kdl[2,6]], [J_kdl[3,0],J_kdl[3,1],J_kdl[3,2],J_kdl[3,3],J_kdl[3,4],J_kdl[3,5],J_kdl[3,6]], [J_kdl[4,0],J_kdl[4,1],J_kdl[4,2],J_kdl[4,3],J_kdl[4,4],J_kdl[4,5],J_kdl[4,6]], [J_kdl[5,0],J_kdl[5,1],J_kdl[5,2],J_kdl[5,3],J_kdl[5,4],J_kdl[5,5],J_kdl[5,6]], ]) return kdl_jac
def Jac_kdl(self, arm, q): J_kdl = kdl.Jacobian(7) if arm != 0: rospy.logerr('Unsupported arm: '+ str(arm)) return None self.right_jac.JntToJac(q,J_kdl) kdl_jac = np.matrix([ [J_kdl[0,0],J_kdl[0,1],J_kdl[0,2],J_kdl[0,3],J_kdl[0,4],J_kdl[0,5],J_kdl[0,6]], [J_kdl[1,0],J_kdl[1,1],J_kdl[1,2],J_kdl[1,3],J_kdl[1,4],J_kdl[1,5],J_kdl[1,6]], [J_kdl[2,0],J_kdl[2,1],J_kdl[2,2],J_kdl[2,3],J_kdl[2,4],J_kdl[2,5],J_kdl[2,6]], [J_kdl[3,0],J_kdl[3,1],J_kdl[3,2],J_kdl[3,3],J_kdl[3,4],J_kdl[3,5],J_kdl[3,6]], [J_kdl[4,0],J_kdl[4,1],J_kdl[4,2],J_kdl[4,3],J_kdl[4,4],J_kdl[4,5],J_kdl[4,6]], [J_kdl[5,0],J_kdl[5,1],J_kdl[5,2],J_kdl[5,3],J_kdl[5,4],J_kdl[5,5],J_kdl[5,6]], ]) return kdl_jac
def force_to_torques(self, forces, joint_positions): # TODO test (correct reference frame?????) torques = [] i = 0 for f, j in zip(forces, joint_positions): jacobian = kdl.Jacobian(3) joints = kdl.JntArray(3) joints[0] = joint_positions[i + 0] joints[1] = joint_positions[i + 1] joints[2] = joint_positions[i + 2] self.jac_list[i].JntToJac(joints, jacobian) jacobian = rm.jac_to_np(jacobian) f.reshape(3, 1) f = np.vstack([f.reshape(3, 1), np.zeros((3, 1))]) t = (jacobian.T).dot(f) torques.append(t) i += 1 return torques
def get_jacobian(self, joint_angles): """ Return the geometric jacobian on the given joint angles. Refer to P112 in "Robotics: Modeling, Planning, and Control" :param joint_angles: joint_angles :type joint_angles: list or flattened np.ndarray :return: jacobian :rtype: np.ndarray """ q = kdl.JntArray(self.urdf_chain.getNrOfJoints()) for i in range(q.rows()): q[i] = joint_angles[i] jac = kdl.Jacobian(self.urdf_chain.getNrOfJoints()) fg = self.jac_solver.JntToJac(q, jac) assert fg == 0, 'KDL JntToJac error!' jac_np = prutil.kdl_array_to_numpy(jac) return jac_np
def get_jacobian(self, joint_angles): """ Return the geometric jacobian on the given joint angles. Refer to P112 in "Robotics: Modeling, Planning, and Control". Args: joint_angles (list or flattened np.ndarray): joint angles. Returns: np.ndarray: jacobian (shape: :math:`[6, DOF]`). """ q = kdl.JntArray(self._urdf_chain.getNrOfJoints()) for i in range(q.rows()): q[i] = joint_angles[i] jac = kdl.Jacobian(self._urdf_chain.getNrOfJoints()) fg = self._jac_solver.JntToJac(q, jac) if fg < 0: raise ValueError('KDL JntToJac error!') jac_np = kdl_array_to_numpy(jac) return jac_np
def __init__(self, urdf, world='world', tip='tip'): # Try loading the URDF data into a KDL tree. (ok, self.tree) = kdlp.treeFromString(urdf) assert ok, "Unable to parse the URDF" # Create the isolated chain from world to tip. self.chain = self.tree.getChain(world, tip) # Extract the number of joints. self.N = self.chain.getNrOfJoints() assert self.N > 0, "Found no joints in the chain" # Create storage for the joint position, tip position, and # Jacobian matrices (for the KDL library). self.qkdl = kdl.JntArray(self.N) self.Tkdl = kdl.Frame() self.Jkdl = kdl.Jacobian(self.N) # Instantiate the solvers for tip position and Jacobian. self.fkinsolver = kdl.ChainFkSolverPos_recursive(self.chain) self.jacsolver = kdl.ChainJntToJacSolver(self.chain)
def force_to_torques(self, forces, joint_positions): # TODO test (correct reference frame?????) torques = [] for i in range(4): # print(f) jacobian = kdl.Jacobian(3) joints = kdl.JntArray(3) joints[0] = joint_positions[3 * i + 0] joints[1] = joint_positions[3 * i + 1] joints[2] = joint_positions[3 * i + 2] # print(joints) self.jac_list[i].JntToJac(joints, jacobian) jacobian = rm.jac_to_np(jacobian) f = np.array( [forces[3 * i + 0], forces[3 * i + 1], forces[3 * i + 2]]) f = np.vstack([f.reshape(3, 1), np.zeros((3, 1))]) # flip torque, want let to apply downwards force t = (jacobian.T).dot(-f) torques.append(t[:, 0]) i += 1 return np.concatenate(torques)
def __init__(self, robotDescriptionString=None, arm_id='panda'): if robotDescriptionString is None: self.urdf_string = _rospy.get_param('robot_description') self.arm_id = _rospy.get_param('arm_id') else: self.urdf_string = robotDescriptionString self.arm_id = arm_id self.baseLinkName = "{}_link0".format(self.arm_id) self.eeLinkName = "{}_link7".format(self.arm_id) isSuccessful, self.kdltree = _kdl_parser.treeFromString( self.urdf_string) if not isSuccessful: raise RuntimeError("could not parse 'robot_description'") self.ee_chain = self.kdltree.getChain(self.baseLinkName, self.eeLinkName) self.fk_ee = _kdl.ChainFkSolverPos_recursive(self.ee_chain) self.jointposition = _kdl.JntArray(7) self.jointvelocity = _kdl.JntArray(7) self.eeFrame = _kdl.Frame() # Calculate the jacobian expressed in the base frame of the chain, with reference point at the end effector of the *chain. # http://docs.ros.org/hydro/api/orocos_kdl/html/classKDL_1_1ChainJntToJacSolver.html#details # Sounds like base jacobian but will be used here for both self.jac_ee = _kdl.ChainJntToJacSolver(self.ee_chain) self.jacobian = _kdl.Jacobian(7) #dynamics: (needs masses added to urdf!) self.grav_vector = _kdl.Vector(0., 0., -9.81) self.dynParam = _kdl.ChainDynParam(self.ee_chain, self.grav_vector) self.inertiaMatrix_kdl = _kdl.JntSpaceInertiaMatrix(7) self.inertiaMatrix = _np.eye((8)) self.gravity_torques_kdl = _kdl.JntArray(7) self.gravity_torques = _np.zeros((8)) viscuousFriction = [1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5] #TODO: load from URDF self.viscuousFriction = _np.array(viscuousFriction)
def compute_jacobian(self, joint_angles_kdl_array): '''Function to compute the Jacobian at a particular set of joint angles''' jacobian = PyKDL.Jacobian(self.num_joints) self.jacobian_solver.JntToJac(joint_angles_kdl_array, jacobian) jacobian_matrix = self.kdl_array_to_numpy_mat(jacobian) return jacobian_matrix
def jacobian(self): jacobian = PyKDL.Jacobian(self._num_jnts) self._jac_kdl.JntToJac(self.joints_to_kdl('positions'), jacobian) return self.kdl_to_mat(jacobian)
def get_jacobian(self, joint): jac = PyKDL.Jacobian(self.kine_chain.getNrOfJoints()) self.jac_calc.JntToJac(joint, jac) return jac
def __init__(self, freq_control=100, margin_workspace=0.05): # Load robot urdf_model = URDF.from_parameter_server() fetch = kdl_tree_from_urdf_model(urdf_model) fetch_arm = fetch.getChain(BASE_LINK, END_LINK) self.dof = fetch_arm.getNrOfJoints() self.kdl_pos = PyKDL.ChainFkSolverPos_recursive(fetch_arm) self.kdl_jac = PyKDL.ChainJntToJacSolver(fetch_arm) self.kdl_dyn = PyKDL.ChainDynParam(fetch_arm, PyKDL.Vector(0, 0, -9.81)) self.kdl_q = PyKDL.JntArray(self.dof) self.kdl_A = PyKDL.JntSpaceInertiaMatrix(self.dof) self.kdl_J = PyKDL.Jacobian(self.dof) self.kdl_x = PyKDL.Frame() # self.kdl_G = PyKDL.JntArray(self.dof) # self.G = np.zeros((self.dof,)) # Initialize robot values self.lock = threading.Lock() self.thread_q = np.zeros((self.dof, )) self.thread_dq = np.zeros((self.dof, )) self.thread_tau = np.zeros((self.dof, )) self.q = np.zeros((self.dof, )) self.dq = np.zeros((self.dof, )) self.tau = np.zeros((self.dof, )) self.x = np.zeros((3, )) self.quat = np.array([0., 0., 0., 1.]) self.lim_norm_x = self.compute_workspace() - margin_workspace self.A = np.zeros((self.dof, self.dof)) self.A_inv = np.zeros((self.dof, self.dof)) self.J = np.zeros((6, self.dof)) self.q_init = np.array([ 0., -np.pi / 4., 0., np.pi / 4, 0., np.pi / 2, 0., ]) # Face down self.q_tuck = np.array([1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]) # Storage position self.q_stow = np.array([1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]) self.q_intermediate_stow = np.array( [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]) self.x_des = np.array([0.8, 0., 0.35]) # q_init self.x_init = np.array([0.8, 0., 0.35]) # q_init # self.quat_des = np.array([-0.707, 0., 0.707, 0.]) # Face up self.quat_des = np.array([0., 0.707, 0., 0.707]) # Face down self.state = "INITIALIZE" print("Switching to state: " + self.state) self.t_start = time.time() self.freq_control = freq_control # Initialize pub and sub self.pub = rospy.Publisher("arm_controller/joint_torque/command", Float64MultiArray, queue_size=1) sub = rospy.Subscriber( "joint_states", JointState, lambda joint_states: self.read_joint_sensors(joint_states)) # Initialize ROS rospy.init_node("joint_space_controller")
def __init__(self, chain): self.chain = chain self.fksolver = PyKDL.ChainFkSolverPos_recursive(self.chain) self.jac_solver = PyKDL.ChainJntToJacSolver(self.chain) self.jacobian = PyKDL.Jacobian(self.chain.getNrOfJoints()) self.joints = self.get_joints()
def spin(self): rospack = rospkg.RosPack() urdf_path = rospack.get_path( 'planar_manipulator_defs') + '/robots/planar_manipulator.urdf' srdf_path = rospack.get_path( 'planar_manipulator_defs') + '/robots/planar_manipulator.srdf' dyn_model = planar5.DynModelPlanar5() col = collision_model.CollisionModel() col.readUrdfSrdf(urdf_path, srdf_path) self.joint_names = [ "0_joint", "1_joint", "2_joint", "3_joint", "4_joint" ] effector_name = 'effector' ndof = len(self.joint_names) # robot state self.q = np.zeros(ndof) self.dq = np.zeros(ndof) self.q[0] = 0.2 self.q[1] = -0.1 self.q[2] = -0.1 self.q[3] = -0.1 self.q[4] = -0.1 solver = fk_ik.FkIkSolver(self.joint_names, [], None) self.publishJointState() # obstacles obst = [] obj = collision_model.CollisionModel.Collision() obj.type = "capsule" obj.T_L_O = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0 / 180.0 * math.pi), PyKDL.Vector(1.5, 0.7, 0)) obj.radius = 0.1 obj.length = 0.4 obst.append(obj) obj = collision_model.CollisionModel.Collision() obj.type = "capsule" obj.T_L_O = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0 / 180.0 * math.pi), PyKDL.Vector(1.2, -0.9, 0)) obj.radius = 0.1 obj.length = 0.4 obst.append(obj) obj = collision_model.CollisionModel.Collision() obj.type = "sphere" obj.T_L_O = PyKDL.Frame(PyKDL.Vector(1, -0.2, 0)) obj.radius = 0.05 obst.append(obj) last_time = rospy.Time.now() limit_range = np.zeros(ndof) max_trq = np.zeros(ndof) for q_idx in range(ndof): limit_range[q_idx] = 15.0 / 180.0 * math.pi max_trq[q_idx] = 10.0 dyn_model.computeM(self.q) obst_offset = 0.0 counter = 10000 while not rospy.is_shutdown(): obst_offset += 0.0001 obst[-1].T_L_O = PyKDL.Frame( PyKDL.Vector(1, -0.2 + math.sin(obst_offset), 0)) if counter > 800: r_HAND_target = PyKDL.Frame( PyKDL.Rotation.RotZ(random.uniform(-math.pi, math.pi)), PyKDL.Vector(random.uniform(0, 2), random.uniform(-1, 1), 0)) qt = r_HAND_target.M.GetQuaternion() pt = r_HAND_target.p print "**** STATE ****" print "r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s,%s,%s,%s), PyKDL.Vector(%s,%s,%s))" % ( qt[0], qt[1], qt[2], qt[3], pt[0], pt[1], pt[2]) print "self.q = np.array(", self.q, ")" print "self.dq = np.array(", self.dq, ")" counter = 0 counter += 1 time_elapsed = rospy.Time.now() - last_time # # mass matrix # dyn_model.computeM(self.q) M = dyn_model.M Minv = dyn_model.Minv # # JLC # torque_JLC = np.zeros(ndof) K = np.zeros((ndof, ndof)) for q_idx in range(ndof): torque_JLC[q_idx] = self.jointLimitTrq(solver.lim_upper[q_idx], solver.lim_lower[q_idx], limit_range[q_idx], max_trq[q_idx], self.q[q_idx]) if abs(torque_JLC[q_idx]) > 0.001: K[q_idx, q_idx] = max_trq[q_idx] / limit_range[q_idx] else: K[q_idx, q_idx] = 0.001 w, v = scipy.linalg.eigh(a=K, b=M) # q_ = dyn_model.gaussjordan(np.matrix(v)) q_ = np.linalg.inv(np.matrix(v)) k0_ = w k0_sqrt = np.zeros(k0_.shape) for q_idx in range(ndof): k0_sqrt[q_idx] = math.sqrt(k0_[q_idx]) tmpNN_ = np.diag(k0_sqrt) D = 2.0 * q_.H * 0.7 * tmpNN_ * q_ torque_JLC_mx = -D * np.matrix(self.dq).transpose() for q_idx in range(ndof): torque_JLC[q_idx] += torque_JLC_mx[q_idx, 0] # calculate jacobian (the activation function) J_JLC = np.matrix(numpy.zeros((ndof, ndof))) for q_idx in range(ndof): if self.q[q_idx] < solver.lim_lower_soft[q_idx]: J_JLC[q_idx, q_idx] = min( 1.0, 10 * abs(self.q[q_idx] - solver.lim_lower_soft[q_idx]) / abs(solver.lim_lower[q_idx] - solver.lim_lower_soft[q_idx])) elif self.q[q_idx] > solver.lim_upper_soft[q_idx]: J_JLC[q_idx, q_idx] = min( 1.0, 10 * abs(self.q[q_idx] - solver.lim_upper_soft[q_idx]) / abs(solver.lim_upper[q_idx] - solver.lim_upper_soft[q_idx])) else: J_JLC[q_idx, q_idx] = 0.0 N_JLC = np.matrix(np.identity(ndof)) - (J_JLC.transpose() * J_JLC) links_fk = {} for link in col.links: links_fk[link.name] = solver.calculateFk( 'base', link.name, self.q) # # collision constraints # link_collision_map = {} if True: activation_dist = 0.05 # self collision total_contacts = 0 for link1_name, link2_name in col.collision_pairs: link1 = col.link_map[link1_name] T_B_L1 = links_fk[link1_name] link2 = col.link_map[link2_name] T_B_L2 = links_fk[link2_name] for col1 in link1.col: for col2 in link2.col: T_B_C1 = T_B_L1 * col1.T_L_O T_B_C2 = T_B_L2 * col2.T_L_O c_dist = (T_B_C1 * PyKDL.Vector() - T_B_C2 * PyKDL.Vector()).Norm() if col1.type == "capsule": c_dist -= col1.radius + col1.length / 2 elif col1.type == "sphere": c_dist -= col1.radius if col2.type == "capsule": c_dist -= col2.radius + col2.length / 2 elif col2.type == "sphere": c_dist -= col2.radius if c_dist > activation_dist: continue dist = None if col1.type == "capsule" and col2.type == "capsule": line1 = (T_B_C1 * PyKDL.Vector(0, -col1.length / 2, 0), T_B_C1 * PyKDL.Vector(0, col1.length / 2, 0)) line2 = (T_B_C2 * PyKDL.Vector(0, -col2.length / 2, 0), T_B_C2 * PyKDL.Vector(0, col2.length / 2, 0)) dist, p1_B, p2_B = self.distanceLines( line1, line2) elif col1.type == "capsule" and col2.type == "sphere": line = (T_B_C1 * PyKDL.Vector(0, -col1.length / 2, 0), T_B_C1 * PyKDL.Vector(0, col1.length / 2, 0)) pt = T_B_C2 * PyKDL.Vector() dist, p1_B, p2_B = self.distanceLinePoint( line, pt) elif col1.type == "sphere" and col2.type == "capsule": pt = T_B_C1 * PyKDL.Vector() line = (T_B_C2 * PyKDL.Vector(0, -col2.length / 2, 0), T_B_C2 * PyKDL.Vector(0, col2.length / 2, 0)) dist, p1_B, p2_B = self.distancePointLine( pt, line) elif col1.type == "sphere" and col2.type == "sphere": dist, p1_B, p2_B = self.distancePoints( T_B_C1 * PyKDL.Vector(), T_B_C2 * PyKDL.Vector()) else: print "ERROR: unknown collision type:", col1.type, col2.type exit(0) if dist != None: dist -= col1.radius + col2.radius v = p2_B - p1_B v.Normalize() n1_B = v n2_B = -v p1_B += n1_B * col1.radius p2_B += n2_B * col2.radius if dist < activation_dist: if not (link1_name, link2_name) in link_collision_map: link_collision_map[(link1_name, link2_name)] = [] link_collision_map[(link1_name, link2_name)].append( (p1_B, p2_B, dist, n1_B, n2_B)) # environment collisions for link in col.links: if link.col == None: continue link1_name = link.name T_B_L1 = links_fk[link1_name] T_B_L2 = links_fk["base"] for col1 in link.col: for col2 in obst: T_B_C1 = T_B_L1 * col1.T_L_O T_B_C2 = T_B_L2 * col2.T_L_O dist = None if col1.type == "capsule" and col2.type == "capsule": line1 = (T_B_C1 * PyKDL.Vector(0, -col1.length / 2, 0), T_B_C1 * PyKDL.Vector(0, col1.length / 2, 0)) line2 = (T_B_C2 * PyKDL.Vector(0, -col2.length / 2, 0), T_B_C2 * PyKDL.Vector(0, col2.length / 2, 0)) dist, p1_B, p2_B = self.distanceLines( line1, line2) elif col1.type == "capsule" and col2.type == "sphere": line = (T_B_C1 * PyKDL.Vector(0, -col1.length / 2, 0), T_B_C1 * PyKDL.Vector(0, col1.length / 2, 0)) pt = T_B_C2 * PyKDL.Vector() dist, p1_B, p2_B = self.distanceLinePoint( line, pt) elif col1.type == "sphere" and col2.type == "capsule": pt = T_B_C1 * PyKDL.Vector() line = (T_B_C2 * PyKDL.Vector(0, -col2.length / 2, 0), T_B_C2 * PyKDL.Vector(0, col2.length / 2, 0)) dist, p1_B, p2_B = self.distancePointLine( pt, line) elif col1.type == "sphere" and col2.type == "sphere": dist, p1_B, p2_B = self.distancePoints( T_B_C1 * PyKDL.Vector(), T_B_C2 * PyKDL.Vector()) else: print "ERROR: unknown collision type:", col1.type, col2.type exit(0) if dist != None: dist -= col1.radius + col2.radius v = p2_B - p1_B v.Normalize() n1_B = v n2_B = -v p1_B += n1_B * col1.radius p2_B += n2_B * col2.radius if dist < activation_dist: if not (link1_name, "base") in link_collision_map: link_collision_map[(link1_name, "base")] = [] link_collision_map[(link1_name, "base")].append( (p1_B, p2_B, dist, n1_B, n2_B)) torque_col = np.matrix(np.zeros((ndof, 1))) Ncol = np.matrix(np.identity(ndof)) m_id = 0 for link1_name, link2_name in link_collision_map: for (p1_B, p2_B, dist, n1_B, n2_B) in link_collision_map[(link1_name, link2_name)]: if dist > 0.0: m_id = self.pub_marker.publishVectorMarker( p1_B, p2_B, m_id, 1, 1, 1, frame='base', namespace='default', scale=0.02) else: m_id = self.pub_marker.publishVectorMarker( p1_B, p2_B, m_id, 1, 0, 0, frame='base', namespace='default', scale=0.02) T_B_L1 = links_fk[link1_name] T_L1_B = T_B_L1.Inverse() T_B_L2 = links_fk[link2_name] T_L2_B = T_B_L2.Inverse() p1_L1 = T_L1_B * p1_B p2_L2 = T_L2_B * p2_B n1_L1 = PyKDL.Frame(T_L1_B.M) * n1_B n2_L2 = PyKDL.Frame(T_L2_B.M) * n2_B # print p1_L1, p1_L1+n1_L1*0.2 # print p2_L2, p2_L2+n2_L2*0.2 # m_id = self.pub_marker.publishVectorMarker(p1_L1, p1_L1+n1_L1*0.2, m_id, 0, 1, 0, frame=link1_name, namespace='default', scale=0.02) # m_id = self.pub_marker.publishVectorMarker(T_B_L2*p2_L2, T_B_L2*(p2_L2+n2_L2*0.2), m_id, 0, 0, 1, frame="base", namespace='default', scale=0.02) jac1 = PyKDL.Jacobian(ndof) jac2 = PyKDL.Jacobian(ndof) common_link_name = solver.getJacobiansForPairX( jac1, jac2, link1_name, p1_L1, link2_name, p2_L2, self.q, None) # print link1_name, link2_name depth = (activation_dist - dist) # repulsive force Fmax = 20.0 if dist <= activation_dist: f = (dist - activation_dist) / activation_dist else: f = 0.0 Frep = Fmax * f * f K = 2.0 * Fmax / (activation_dist * activation_dist) # the mapping between motions along contact normal and the Cartesian coordinates e1 = n1_L1 e2 = n2_L2 Jd1 = np.matrix([e1[0], e1[1], e1[2]]) Jd2 = np.matrix([e2[0], e2[1], e2[2]]) # rewrite the linear part of the jacobian jac1_mx = np.matrix(np.zeros((3, ndof))) jac2_mx = np.matrix(np.zeros((3, ndof))) for q_idx in range(ndof): col1 = jac1.getColumn(q_idx) col2 = jac2.getColumn(q_idx) for row_idx in range(3): jac1_mx[row_idx, q_idx] = col1[row_idx] jac2_mx[row_idx, q_idx] = col2[row_idx] Jcol1 = Jd1 * jac1_mx Jcol2 = Jd2 * jac2_mx Jcol = np.matrix(np.zeros((1, ndof))) for q_idx in range(ndof): Jcol[0, q_idx] = Jcol1[0, q_idx] + Jcol2[0, q_idx] # calculate relative velocity between points ddij = Jcol * np.matrix(self.dq).transpose() activation = min(1.0, 5.0 * depth / activation_dist) activation = max(0.0, activation) if ddij <= 0.0: activation = 0.0 a_des = activation # print "activation", activation # raw_input(".") if rospy.is_shutdown(): exit(0) Ncol12 = np.matrix( np.identity(ndof)) - (Jcol.transpose() * a_des * Jcol) Ncol = Ncol * Ncol12 # calculate collision mass Mdij = Jcol * Minv * Jcol.transpose() D = 2.0 * 0.7 * math.sqrt(Mdij * K) d_torque = Jcol.transpose() * (-Frep - D * ddij) torque_col += d_torque self.pub_marker.eraseMarkers(m_id, 10, frame_id='base', namespace='default') # # end-effector task # T_B_E = links_fk[effector_name] r_HAND_current = T_B_E diff = PyKDL.diff(r_HAND_current, r_HAND_target) r_HAND_diff = np.array([diff[0], diff[1], diff[5]]) Kc = np.array([10, 10, 1]) Dxi = np.array([0.7, 0.7, 0.7]) wrench = np.zeros(3) for dim in range(3): wrench[dim] = Kc[dim] * r_HAND_diff[dim] J_r_HAND_6 = solver.getJacobian('base', effector_name, self.q) J_r_HAND = np.matrix(np.zeros((3, 5))) for q_idx in range(ndof): J_r_HAND[0, q_idx] = J_r_HAND_6[0, q_idx] J_r_HAND[1, q_idx] = J_r_HAND_6[1, q_idx] J_r_HAND[2, q_idx] = J_r_HAND_6[5, q_idx] torque_HAND = J_r_HAND.transpose() * np.matrix(wrench).transpose() A = J_r_HAND * Minv * J_r_HAND.transpose() # A = dyn_model.gaussjordan(A) A = np.linalg.inv(A) tmpKK_ = np.matrix(np.diag(Kc)) w, v = scipy.linalg.eigh(a=tmpKK_, b=A) # Q = dyn_model.gaussjordan(np.matrix(v)) Q = np.linalg.inv(np.matrix(v)) K0 = w Dc = Q.transpose() * np.matrix(np.diag(Dxi)) K0_sqrt = np.zeros(K0.shape) for dim_idx in range(3): K0_sqrt[dim_idx] = math.sqrt(K0[dim_idx]) Dc = 2.0 * Dc * np.matrix(np.diag(K0_sqrt)) * Q F = Dc * J_r_HAND * np.matrix(self.dq).transpose() torque_HAND_mx = -J_r_HAND.transpose() * F for q_idx in range(ndof): torque_HAND[q_idx] += torque_HAND_mx[q_idx, 0] # null-space # tmpNK_.noalias() = J * Mi; # tmpKK_.noalias() = tmpNK_ * JT; # luKK_.compute(tmpKK_); # tmpKK_ = luKK_.inverse(); # tmpKN_.noalias() = Mi * JT; # Ji.noalias() = tmpKN_ * tmpKK_; # # P.noalias() = Eigen::MatrixXd::Identity(P.rows(), P.cols()); # P.noalias() -= J.transpose() * A * J * Mi; torque = np.matrix(torque_JLC).transpose() + N_JLC.transpose() * ( torque_col + (Ncol.transpose() * torque_HAND)) # torque = torque_HAND time_d = 0.01 # simulate one step ddq = dyn_model.accel(self.q, self.dq, torque) for q_idx in range(ndof): self.dq[q_idx] += ddq[q_idx, 0] * time_d self.q[q_idx] += self.dq[q_idx] * time_d if time_elapsed.to_sec() > 0.05: m_id = 0 m_id = self.publishRobotModelVis(m_id, col, links_fk) m_id = self.publishObstaclesVis(m_id, obst) last_time = rospy.Time.now() self.publishJointState() self.publishTransform(r_HAND_target, "hand_dest")
#求取坐标的逆 frame_inv = f3.Inverse() print "frame_inv:\n", frame_inv #create a wrench wr = PyKDL.Wrench() wr.force = PyKDL.Vector(0, 1, 2) wr.torque = PyKDL.Vector(3, 4, 5) print "wr:\n", wr #read a wrench f = wr.force print "f:", f M = wr.torque print "M:", M #create a Twist Tw = PyKDL.Twist() Tw.vel = PyKDL.Vector(0, 1, 2) Tw.rot = PyKDL.Vector(3, 4, 5) print "Tw:\n", Tw #read a Twist v = Tw.vel print "v:", v w = Tw.rot print "w:", w #create a jacobian jac = PyKDL.Jacobian(7) print "jac", jac.columns()
def spin(self): rospack = rospkg.RosPack() model = "5dof" # model = "6dof" # model = "two_arms" if model == "6dof": urdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator_6dof.urdf' srdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator_6dof.srdf' elif model == "5dof": urdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator.urdf' srdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_manipulator.srdf' elif model == "two_arms": urdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_two_arms.urdf' srdf_path=rospack.get_path('planar_manipulator_defs') + '/robots/planar_two_arms.srdf' # TEST: line - line distance if False: while not rospy.is_shutdown(): pt = PyKDL.Vector(random.uniform(-1, 1), random.uniform(-1, 1), 0) line = (PyKDL.Vector(random.uniform(-1, 1),random.uniform(-1, 1),0), PyKDL.Vector(random.uniform(-1, 1),random.uniform(-1, 1),0)) line2 = (PyKDL.Vector(random.uniform(-1, 1),random.uniform(-1, 1),0), PyKDL.Vector(random.uniform(-1, 1),random.uniform(-1, 1),0)) dist, p1, p2 = self.distanceLines(line, line2) m_id = 0 m_id = self.pub_marker.publishVectorMarker(line[0], line[1], m_id, 0, 1, 0, frame='world', namespace='default', scale=0.02) m_id = self.pub_marker.publishVectorMarker(line2[0], line2[1], m_id, 1, 0, 0, frame='world', namespace='default', scale=0.02) m_id = self.pub_marker.publishVectorMarker(p1, p2, m_id, 1, 1, 1, frame='world', namespace='default', scale=0.02) print line, line2, dist raw_input(".") exit(0) # TEST: point - point distance if False: while not rospy.is_shutdown(): pt1 = PyKDL.Vector(random.uniform(-1, 1), random.uniform(-1, 1), 0) pt2 = PyKDL.Vector(random.uniform(-1, 1), random.uniform(-1, 1), 0) dist, p1, p2 = self.distancePoints(pt1, pt2) m_id = 0 m_id = self.pub_marker.publishSinglePointMarker(p1, m_id, r=0, g=1, b=0, a=1, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.1, 0.1, 0.1), T=None) m_id = self.pub_marker.publishSinglePointMarker(p2, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.1, 0.1, 0.1), T=None) m_id = self.pub_marker.publishVectorMarker(p1, p2, m_id, 1, 0, 0, frame='world', namespace='default', scale=0.02) print pt1, pt2, dist raw_input(".") exit(0) col = collision_model.CollisionModel() col.readUrdfSrdf(urdf_path, srdf_path) if model == "6dof": self.joint_names = ["0_joint", "1_joint", "2_joint", "3_joint", "4_joint", "5_joint"] effector_name = 'effector' elif model == "5dof": self.joint_names = ["0_joint", "1_joint", "2_joint", "3_joint", "4_joint"] effector_name = 'effector' elif model == "two_arms": self.joint_names = ["torso_0_joint", "left_0_joint", "left_1_joint", "left_2_joint", "left_3_joint", "right_0_joint", "right_1_joint", "right_2_joint", "right_3_joint"] effector_name = 'left_effector' self.q = np.zeros( len(self.joint_names) ) test_cases = [ (1.1, -2.3, 1.5, 1.5, 1.5), (-1.1, 2.3, -1.5, -1.5, -1.5), (0.0, 0.0, 1.5, 1.5, 1.5), (0.0, 0.0, -1.5, -1.5, -1.5), (0.2, 0.4, 1.5, 1.5, 1.5), (-0.2, -0.4, -1.5, -1.5, -1.5), ] # self.q[0] = 1.1 # self.q[1] = -2.3 # self.q[2] = 1.5 # self.q[3] = 1.5 # self.q[4] = 1.5 # self.q = np.array(test_cases[4]) solver = fk_ik.FkIkSolver(self.joint_names, [], None) self.publishJointState() # rospy.sleep(1) # exit(0) obst = [] obj = collision_model.CollisionModel.Collision() obj.type = "capsule" obj.T_L_O = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0/180.0*math.pi), PyKDL.Vector(1.5, 0.7, 0)) obj.radius = 0.1 obj.length = 0.4 obst.append( obj ) obj = collision_model.CollisionModel.Collision() obj.type = "capsule" obj.T_L_O = PyKDL.Frame(PyKDL.Rotation.RotZ(90.0/180.0*math.pi), PyKDL.Vector(1.2, -0.9, 0)) obj.radius = 0.1 obj.length = 0.4 obst.append( obj ) obj = collision_model.CollisionModel.Collision() obj.type = "sphere" obj.T_L_O = PyKDL.Frame(PyKDL.Vector(1, 0, 0)) obj.radius = 0.05 obst.append( obj ) # rospy.sleep(1) # T_B_E = solver.calculateFk("base", "effector", self.q) # print T_B_E r_HAND_targets = [ PyKDL.Frame(PyKDL.Vector(0.1,1.0,0.0)), PyKDL.Frame(PyKDL.Vector(0.1,1.7,0.0)), # PyKDL.Frame(PyKDL.Rotation.RotY(90.0/180.0*math.pi), PyKDL.Vector(0.2,-0.5,0.0)), ] target_idx = 0 r_HAND_target = r_HAND_targets[target_idx] target_idx += 1 r_HAND_target = PyKDL.Frame(PyKDL.Vector(0.5,0.5,0)) last_time = rospy.Time.now() counter = 10000 while not rospy.is_shutdown(): if counter > 800: if model == "two_arms": r_HAND_target = PyKDL.Frame(PyKDL.Rotation.RotZ(random.uniform(-math.pi, math.pi)), PyKDL.Vector(random.uniform(-1,0), random.uniform(0,1.8), 0)) else: r_HAND_target = PyKDL.Frame(PyKDL.Rotation.RotZ(random.uniform(-math.pi, math.pi)), PyKDL.Vector(random.uniform(0,2), random.uniform(-1,1), 0)) # r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,-0.98100002989,0.194007580664), PyKDL.Vector(-0.129108034334,0.518606130706,0.0)) # r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.470814280381,0.882232346601), PyKDL.Vector(0.676567476122,0.0206561816531,0.0)) #r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.50451570265,0.863402516663), PyKDL.Vector(0.252380653828,0.923309935287,0.0)) #self.q = np.array( [-0.29968745 0.66939973 -2.49850991 1.87533697 2.63546305] ) #r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.704294768084,0.70990765572), PyKDL.Vector(0.334245569765,1.82368612057,0.0)) #self.q = np.array( [ 0.33203731 0.071835 -2.46646112 1.14339024 1.97684146] ) # r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.924467039084,-0.381261975087), PyKDL.Vector(0.261697539135,0.97235224304,0.0)) #r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.894763681298,0.446539980999), PyKDL.Vector(0.354981453046,0.604598917063,0.0)) #self.q = np.array( [-0.89640518 0.44336642 1.96125279 -1.66533209 -2.19189403] ) qt = r_HAND_target.M.GetQuaternion() pt = r_HAND_target.p print "r_HAND_target = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s,%s,%s,%s), PyKDL.Vector(%s,%s,%s))"%(qt[0],qt[1],qt[2],qt[3],pt[0],pt[1],pt[2]) print "self.q = np.array(", self.q, ")" counter = 0 counter += 1 time_elapsed = rospy.Time.now() - last_time J_JLC = np.matrix(numpy.zeros( (len(self.q), len(self.q)) )) delta_V_JLC = np.empty(len(self.q)) for q_idx in range(len(self.q)): if self.q[q_idx] < solver.lim_lower_soft[q_idx]: delta_V_JLC[q_idx] = self.q[q_idx] - solver.lim_lower_soft[q_idx] J_JLC[q_idx,q_idx] = min(1.0, 10*abs(self.q[q_idx] - solver.lim_lower_soft[q_idx]) / abs(solver.lim_lower[q_idx] - solver.lim_lower_soft[q_idx])) elif self.q[q_idx] > solver.lim_upper_soft[q_idx]: delta_V_JLC[q_idx] = self.q[q_idx] - solver.lim_upper_soft[q_idx] J_JLC[q_idx,q_idx] = min(1.0, 10*abs(self.q[q_idx] - solver.lim_upper_soft[q_idx]) / abs(solver.lim_upper[q_idx] - solver.lim_upper_soft[q_idx])) else: delta_V_JLC[q_idx] = 0.0 J_JLC[q_idx,q_idx] = 0.0 J_JLC_inv = J_JLC.transpose()#np.linalg.pinv(J_JLC) N_JLC = np.matrix(np.identity(len(self.q))) - (J_JLC_inv * J_JLC) N_JLC_inv = np.linalg.pinv(N_JLC) v_max_JLC = 20.0/180.0*math.pi kp_JLC = 10.0 dx_JLC_des = kp_JLC * delta_V_JLC # min(1.0, v_max_JLC/np.linalg.norm(dx_JLC_des)) if v_max_JLC > np.linalg.norm(dx_JLC_des): vv_JLC = 1.0 else: vv_JLC = v_max_JLC/np.linalg.norm(dx_JLC_des) dx_JLC_ref = - vv_JLC * dx_JLC_des J_r_HAND = solver.getJacobian('base', effector_name, self.q, base_end=False) J_r_HAND_inv = np.linalg.pinv(J_r_HAND) T_B_E = solver.calculateFk('base', effector_name, self.q) r_HAND_current = T_B_E r_HAND_diff = PyKDL.diff(r_HAND_current, r_HAND_target) delta_V_HAND = np.empty(6) delta_V_HAND[0] = r_HAND_diff.vel[0] delta_V_HAND[1] = r_HAND_diff.vel[1] delta_V_HAND[2] = r_HAND_diff.vel[2] delta_V_HAND[3] = r_HAND_diff.rot[0] delta_V_HAND[4] = r_HAND_diff.rot[1] delta_V_HAND[5] = r_HAND_diff.rot[2] v_max_HAND = 4.0 kp_HAND = 10.0 dx_HAND_des = kp_HAND * delta_V_HAND if v_max_HAND > np.linalg.norm(dx_HAND_des): vv_HAND = 1.0 else: vv_HAND = v_max_HAND/np.linalg.norm(dx_HAND_des) dx_r_HAND_ref = vv_HAND * dx_HAND_des links_fk = {} for link in col.links: links_fk[link.name] = solver.calculateFk('base', link.name, self.q) activation_dist = 0.05 link_collision_map = {} if True: # self collision total_contacts = 0 for link1_name, link2_name in col.collision_pairs: link1 = col.link_map[link1_name] T_B_L1 = links_fk[link1_name] link2 = col.link_map[link2_name] T_B_L2 = links_fk[link2_name] for col1 in link1.col: for col2 in link2.col: T_B_C1 = T_B_L1 * col1.T_L_O T_B_C2 = T_B_L2 * col2.T_L_O c_dist = (T_B_C1 * PyKDL.Vector() - T_B_C2 * PyKDL.Vector()).Norm() if col1.type == "capsule": c_dist -= col1.radius + col1.length/2 elif col1.type == "sphere": c_dist -= col1.radius if col2.type == "capsule": c_dist -= col2.radius + col2.length/2 elif col2.type == "sphere": c_dist -= col2.radius if c_dist > activation_dist: continue dist = None if col1.type == "capsule" and col2.type == "capsule": line1 = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0)) line2 = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0)) dist, p1_B, p2_B = self.distanceLines(line1, line2) elif col1.type == "capsule" and col2.type == "sphere": line = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0)) pt = T_B_C2 * PyKDL.Vector() dist, p1_B, p2_B = self.distanceLinePoint(line, pt) elif col1.type == "sphere" and col2.type == "capsule": pt = T_B_C1 * PyKDL.Vector() line = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0)) dist, p1_B, p2_B = self.distancePointLine(pt, line) elif col1.type == "sphere" and col2.type == "sphere": dist, p1_B, p2_B = self.distancePoints(T_B_C1 * PyKDL.Vector(), T_B_C2 * PyKDL.Vector()) else: print "ERROR: unknown collision type:", col1.type, col2.type exit(0) if dist != None: # print "dist", dist, link1_name, link2_name, col1.type, col2.type dist -= col1.radius + col2.radius v = p2_B - p1_B v.Normalize() n1_B = v n2_B = -v p1_B += n1_B * col1.radius p2_B += n2_B * col2.radius if dist < activation_dist: if not (link1_name, link2_name) in link_collision_map: link_collision_map[(link1_name, link2_name)] = [] link_collision_map[(link1_name, link2_name)].append( (p1_B, p2_B, dist, n1_B, n2_B) ) # environment collisions for link in col.links: if link.col == None: continue link1_name = link.name T_B_L1 = links_fk[link1_name] T_B_L2 = links_fk["base"] for col1 in link.col: for col2 in obst: T_B_C1 = T_B_L1 * col1.T_L_O T_B_C2 = T_B_L2 * col2.T_L_O dist = None if col1.type == "capsule" and col2.type == "capsule": line1 = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0)) line2 = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0)) dist, p1_B, p2_B = self.distanceLines(line1, line2) elif col1.type == "capsule" and col2.type == "sphere": line = (T_B_C1 * PyKDL.Vector(0, -col1.length/2, 0), T_B_C1 * PyKDL.Vector(0, col1.length/2, 0)) pt = T_B_C2 * PyKDL.Vector() dist, p1_B, p2_B = self.distanceLinePoint(line, pt) elif col1.type == "sphere" and col2.type == "capsule": pt = T_B_C1 * PyKDL.Vector() line = (T_B_C2 * PyKDL.Vector(0, -col2.length/2, 0), T_B_C2 * PyKDL.Vector(0, col2.length/2, 0)) dist, p1_B, p2_B = self.distancePointLine(pt, line) elif col1.type == "sphere" and col2.type == "sphere": dist, p1_B, p2_B = self.distancePoints(T_B_C1 * PyKDL.Vector(), T_B_C2 * PyKDL.Vector()) # print "a:",dist, p1_B, p2_B else: print "ERROR: unknown collision type:", col1.type, col2.type exit(0) if dist != None: dist -= col1.radius + col2.radius v = p2_B - p1_B v.Normalize() n1_B = v n2_B = -v p1_B += n1_B * col1.radius p2_B += n2_B * col2.radius # if col1.type == "sphere" and col2.type == "sphere": # print "b:",dist, p1_B, p2_B if dist < activation_dist: if not (link1_name, "base") in link_collision_map: link_collision_map[(link1_name, "base")] = [] link_collision_map[(link1_name, "base")].append( (p1_B, p2_B, dist, n1_B, n2_B) ) omega_col = np.matrix(np.zeros( (len(self.q),1) )) Ncol = np.matrix(np.identity(len(self.q))) m_id = 0 for link1_name, link2_name in link_collision_map: for (p1_B, p2_B, dist, n1_B, n2_B) in link_collision_map[ (link1_name, link2_name) ]: if dist > 0.0: m_id = self.pub_marker.publishVectorMarker(p1_B, p2_B, m_id, 1, 1, 1, frame='base', namespace='default', scale=0.02) else: m_id = self.pub_marker.publishVectorMarker(p1_B, p2_B, m_id, 1, 0, 0, frame='base', namespace='default', scale=0.02) T_B_L1 = links_fk[link1_name] T_L1_B = T_B_L1.Inverse() T_B_L2 = links_fk[link2_name] T_L2_B = T_B_L2.Inverse() p1_L1 = T_L1_B * p1_B p2_L2 = T_L2_B * p2_B n1_L1 = PyKDL.Frame(T_L1_B.M) * n1_B n2_L2 = PyKDL.Frame(T_L2_B.M) * n2_B # print p1_L1, p1_L1+n1_L1*0.2 # print p2_L2, p2_L2+n2_L2*0.2 # m_id = self.pub_marker.publishVectorMarker(p1_L1, p1_L1+n1_L1*0.2, m_id, 0, 1, 0, frame=link1_name, namespace='default', scale=0.02) # m_id = self.pub_marker.publishVectorMarker(T_B_L2*p2_L2, T_B_L2*(p2_L2+n2_L2*0.2), m_id, 0, 0, 1, frame="base", namespace='default', scale=0.02) jac1 = PyKDL.Jacobian(len(self.q)) jac2 = PyKDL.Jacobian(len(self.q)) common_link_name = solver.getJacobiansForPairX(jac1, jac2, link1_name, p1_L1, link2_name, p2_L2, self.q, None) # T_B_Lc = links_fk[common_link_name] # jac1.changeBase(T_B_Lc.M) # jac2.changeBase(T_B_Lc.M) # T_Lc_L1 = T_B_Lc.Inverse() * T_B_L1 # T_Lc_L2 = T_B_Lc.Inverse() * T_B_L2 # n1_Lc = PyKDL.Frame(T_Lc_L1.M) * n1_L1 # n2_Lc = PyKDL.Frame(T_Lc_L2.M) * n2_L2 # print link1_name, link2_name # print "jac1" # print jac1 # print "jac2" # print jac2 # repulsive velocity V_max = 20.0 dist = max(dist, 0.0) depth = (activation_dist - dist) # Vrep = V_max * depth * depth / (activation_dist * activation_dist) Vrep = V_max * depth / activation_dist # Vrep = -max(Vrep, 0.01) Vrep = -Vrep # # the mapping between motions along contact normal and the Cartesian coordinates e1 = n1_L1 e2 = n2_L2 Jd1 = np.matrix([e1[0], e1[1], e1[2]]) Jd2 = np.matrix([e2[0], e2[1], e2[2]]) # rewrite the linear part of the jacobian jac1_mx = np.matrix(np.zeros( (3, len(self.q)) )) jac2_mx = np.matrix(np.zeros( (3, len(self.q)) )) for q_idx in range(len(self.q)): col1 = jac1.getColumn(q_idx) col2 = jac2.getColumn(q_idx) for row_idx in range(3): jac1_mx[row_idx, q_idx] = col1[row_idx] jac2_mx[row_idx, q_idx] = col2[row_idx] Jcol1 = Jd1 * jac1_mx Jcol2 = Jd2 * jac2_mx Jcol = np.matrix(np.zeros( (2, len(self.q)) )) for q_idx in range(len(self.q)): Jcol[0, q_idx] = Jcol1[0, q_idx] Jcol[1, q_idx] = Jcol2[0, q_idx] # Jcol = Jcol * (Ncol * N_JLC) # TODO: is the transposition ok? Jcol_pinv = np.linalg.pinv(Jcol) # Jcol_pinv = Jcol.transpose() activation = min(1.0, 2.0*depth/activation_dist) a_des = np.matrix(np.zeros( (len(self.q),len(self.q)) )) a_des[0,0] = a_des[1,1] = activation U, S, V = numpy.linalg.svd(Jcol, full_matrices=True, compute_uv=True) # print "activation", activation # print "Jcol" # print Jcol # raw_input(".") if rospy.is_shutdown(): exit(0) # print "V" # print V # print "S" # print S # Ncol12 = np.matrix(np.identity(len(self.q))) - Jcol.transpose() * (Jcol_pinv).transpose() Ncol12 = np.matrix(np.identity(len(self.q))) - (V * a_des * V.transpose()) Ncol = Ncol * Ncol12 # d_omega = Jcol_prec_inv * np.matrix([Vrep, Vrep]).transpose() d_omega = Jcol_pinv * np.matrix([Vrep, Vrep]).transpose() omega_col += d_omega # print "omega_col", omega_col # print dx_HAND_ref # omega_r_HAND = (J_r_HAND_inv * np.matrix(dx_r_HAND_ref).transpose()) self.pub_marker.eraseMarkers(m_id, 10, frame_id='base', namespace='default') Ncol_inv = np.linalg.pinv(Ncol) J_r_HAND_prec = J_r_HAND * (Ncol * N_JLC) J_r_HAND_prec_inv = np.linalg.pinv(J_r_HAND_prec) omega = J_JLC_inv * np.matrix(dx_JLC_ref).transpose() + N_JLC.transpose() * (omega_col + (Ncol.transpose() * J_r_HAND_inv) * np.matrix(dx_r_HAND_ref).transpose()) # omega = J_JLC_inv * np.matrix(dx_JLC_ref).transpose() + N_JLC_inv * (omega_col + (Ncol_inv * J_r_HAND_prec_inv) * np.matrix(dx_r_HAND_ref).transpose()) # omega = J_JLC_inv * np.matrix(dx_JLC_ref).transpose() + np.linalg.pinv(N_JLC) * omega_col # omega = omega_col omega_vector = np.empty(len(self.q)) for q_idx in range(len(self.q)): omega_vector[q_idx] = omega[q_idx][0] max_norm = 0.5 omega_norm = np.linalg.norm(omega_vector) if omega_norm > max_norm: omega_vector *= max_norm / omega_norm self.q += omega_vector * 0.02 if time_elapsed.to_sec() > 0.05: # print self.q m_id = 0 m_id = self.publishRobotModelVis(m_id, col, links_fk) m_id = self.publishObstaclesVis(m_id, obst) last_time = rospy.Time.now() self.publishJointState() self.publishTransform(r_HAND_target, "hand_dest")
def jacobian(self,joint_values=None): jacobian = PyKDL.Jacobian(self._num_jnts) self._jac_kdl.JntToJac(self.joints_to_kdl('positions',joint_values), jacobian) return kdl_to_mat(jacobian)
def compute_jacobian(self, joint_angles_kdl_array): jacobian = PyKDL.Jacobian(self.num_joints) self.jacobian_solver.JntToJac(joint_angles_kdl_array, jacobian) jacobian_matrix = self.kdl_array_to_numpy_mat(jacobian) return jacobian_matrix