Exemplo n.º 1
0
    def get_kdl_chain_lwr(self):
        from PyKDL import Frame, Rotation, Vector, Joint, Chain, Segment
        from math import pi
        # arm segments that behave exactly like the DLR system. (Last pose in the middle of the potato)
        #Some parts of the DLR system take into account the TCP_T_EE transformation, for example to the Flange
        arm_segments = [
            Segment(Joint(Joint.None),
                    Frame(Rotation.Quaternion(0.428, 0.261, -0.435, 0.748), Vector(0.222, -0.339, 0.949))),
            Segment(Joint(Joint.None),
                    Frame(Rotation.Identity(), Vector(0.0, 0.0, 0.11))),
            Segment(Joint(Joint.RotZ),
                    Frame(Rotation.RotX(-pi/2), Vector(0.0, 0.0, 0.20))),
            Segment(Joint(Joint.RotZ, -1),
                    Frame(Rotation.RotX(pi/2), Vector(0.0, -0.20, 0.0))),
            Segment(Joint(Joint.RotZ),
                    Frame(Rotation.RotX(pi/2), Vector(0, 0, .20))),
            Segment(Joint(Joint.RotZ, -1),
                    Frame(Rotation.RotX(-pi/2), Vector(0, 0.2, 0))),
            Segment(Joint(Joint.RotZ),
                    Frame(Rotation.RotX(-pi/2), Vector(0, 0, 0.19))),
            Segment(Joint(Joint.RotZ, -1),
                    Frame(Rotation.RotX(pi/2), Vector(0, 0, 0.0))),  #-0.078
            Segment(Joint(Joint.RotZ),
                    Frame(Rotation.Identity(), Vector(0, 0, 0.0))),
        ]


        chain = Chain()
        for segment in arm_segments:
            chain.addSegment(segment)
        return(chain)
Exemplo n.º 2
0
    def build_chain(self):
        self.chain = Chain()
        for e in self._config:
            v = Vector(e["xyzrpy"][0], e["xyzrpy"][1], e["xyzrpy"][2])
            r = Rotation.RPY(e["xyzrpy"][3], e["xyzrpy"][4], e["xyzrpy"][5])
            j = Joint(e["name"], Joint.None)

            f = Frame(r, v)

            if e["type"] == "rotx":
                axis = Vector(1.0, 0.0, 0.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
            elif e["type"] == "roty":
                axis = Vector(0.0, 1.0, 0.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
            elif e["type"] == "rotz":
                axis = Vector(0.0, 0.0, 1.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
            elif e["type"] == "transx":
                axis = Vector(1.0, 0.0, 0.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
            elif e["type"] == "transy":
                axis = Vector(0.0, 1.0, 0.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
            elif e["type"] == "transz":
                axis = Vector(0.0, 0.0, 1.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)

            self.chain.addSegment(
                Segment(e["name"] + "seg", j, f, RigidBodyInertia()))
        self.fksolverpos = ChainFkSolverPos_recursive(self.chain)
Exemplo n.º 3
0
    def compute_effector_position(self, msg):
        positions = JntArray(3)
        chain = Chain()
        kdl_frame = Frame()

        d = 0
        theta = 0

        order = [1, 2, 0]

        for i in order:
            joint = self.params[i]
            a = joint["a"]
            d = joint["d"]
            alpha = joint["alpha"]
            theta = joint["theta"]

            frame = kdl_frame.DH(a, alpha, d, theta)
            chain.addSegment(Segment(Joint(Joint.RotZ), frame))
            positions[i] = msg.position[i]

        fk_solver = ChainFkSolverPos_recursive(chain)
        result = Frame()
        fk_solver.JntToCart(positions, result)

        kdl_pose = PoseStamped()
        kdl_pose.header.frame_id = 'base_link'
        kdl_pose.header.stamp = rospy.Time.now()

        kdl_pose.pose.position.x = result.p[0]
        kdl_pose.pose.position.y = result.p[1]
        kdl_pose.pose.position.z = result.p[2]
        quat = result.M.GetQuaternion()
        kdl_pose.pose.orientation.x = quat[0]
        kdl_pose.pose.orientation.y = quat[1]
        kdl_pose.pose.orientation.z = quat[2]
        kdl_pose.pose.orientation.w = quat[3]
        return kdl_pose
Exemplo n.º 4
0
    def __init__(self,parent,handedness):
        '''parent is the finger parent'''

        #kinematics
        self.parent=parent
        self.config_hand=self.parent.config_hand
        ch=self.config_hand
        #from hands_kin import hands_kin, fingers_lim, fingers_coupling, motor_locked_joints, sensor_locked_joints, sensor_finger_coupling
        from PyKDL import Chain,ChainFkSolverPos_recursive,JntArray,Frame
        from PyKDL import ChainFkSolverVel_recursive,FrameVel
        from PyKDL import ChainJntToJacSolver, Jacobian, JntArrayVel
        from PyKDL import ChainIkSolverVel_wdls, Twist
        self.segments=ch.hands_kin[handedness][self.parent.num_finger]
        self.limits=ch.fingers_lim[self.parent.num_finger]
        self.coupling_matrix=ch.fingers_coupling[self.parent.num_finger]
        self.sensor_finger_coupling=ch.sensor_finger_coupling
        self.motor_locked_joints=ch.motor_locked_joints[self.parent.num_finger]
        self.sensor_locked_joints=ch.sensor_locked_joints[self.parent.num_finger]
        print "Motor_locked", self.motor_locked_joints
        print "Sensor_locked", self.sensor_locked_joints

        #chain motor
        self.motor_chain=Chain()
        for segment in self.segments:
            self.motor_chain.addSegment(segment)
        self.motor_chain.setCoupling(self.motor_locked_joints,self.coupling_matrix.tolist())

        #chain sensor
        self.sensor_chain=Chain(self.motor_chain)
        self.sensor_chain.setCoupling(self.sensor_locked_joints,self.sensor_finger_coupling.tolist())

        #pos fk
        self.pfk_solver=ChainFkSolverPos_recursive(self.motor_chain)
        self.cur_jpos_kdl=JntArray(self.motor_chain.getNrOfJoints())
        self.cur_frame_kdl=Frame()

        #vel fk
        self.vfk_solver=ChainFkSolverVel_recursive(self.motor_chain)
        self.cur_jvel_kdl=JntArrayVel(self.motor_chain.getNrOfJoints()) #TODO for the thumb we don't have joint velocity yet, it has to be calculated.
        self.cur_framevel_kdl=FrameVel()

        #vel ifk
        from numpy import finfo, double
        self.smallest=finfo(double).eps
        self.vik_solver=ChainIkSolverVel_wdls(self.motor_chain,self.smallest)
        #TODO: make set weights methods
        self.qdot_out_kdl=JntArray(self.motor_chain.getNrOfUnlockedJoints())
        self.tw=Twist()

        #jac solver
        self.jac_solver_sensors=ChainJntToJacSolver(self.sensor_chain)
        self.jac_solver_sensors
        self.jac_sensors=Jacobian(self.sensor_chain.getNrOfIndJoints())
        self.forces=array([0.0]*6)

        #finger calibration values
        #angle offsets in radians of the finger joints
        self.base_angle_offset=0.0
        self.proximal_angle_offset=0.0
        self.distal_angle_offset=0.0
        self.distal2_angle_offset=0.0

        #weight matrices for the statics inverse
        self.static_weight_left=diag([1.0, 1.0, 1.0, 0.0, 0.0, 0.0])
        self.static_weight_right=identity(self.sensor_chain.getNrOfIndJoints())
        self.static_weight_right=identity(self.sensor_chain.getNrOfUnlockedJoints())
Exemplo n.º 5
0
class Kinematics(object):
    '''This deals with the finger kinematics, forces, speeds, and positions in cartesian are here calculated, also the inverses of this'''
    def __init__(self,parent,handedness):
        '''parent is the finger parent'''

        #kinematics
        self.parent=parent
        self.config_hand=self.parent.config_hand
        ch=self.config_hand
        #from hands_kin import hands_kin, fingers_lim, fingers_coupling, motor_locked_joints, sensor_locked_joints, sensor_finger_coupling
        from PyKDL import Chain,ChainFkSolverPos_recursive,JntArray,Frame
        from PyKDL import ChainFkSolverVel_recursive,FrameVel
        from PyKDL import ChainJntToJacSolver, Jacobian, JntArrayVel
        from PyKDL import ChainIkSolverVel_wdls, Twist
        self.segments=ch.hands_kin[handedness][self.parent.num_finger]
        self.limits=ch.fingers_lim[self.parent.num_finger]
        self.coupling_matrix=ch.fingers_coupling[self.parent.num_finger]
        self.sensor_finger_coupling=ch.sensor_finger_coupling
        self.motor_locked_joints=ch.motor_locked_joints[self.parent.num_finger]
        self.sensor_locked_joints=ch.sensor_locked_joints[self.parent.num_finger]
        print "Motor_locked", self.motor_locked_joints
        print "Sensor_locked", self.sensor_locked_joints

        #chain motor
        self.motor_chain=Chain()
        for segment in self.segments:
            self.motor_chain.addSegment(segment)
        self.motor_chain.setCoupling(self.motor_locked_joints,self.coupling_matrix.tolist())

        #chain sensor
        self.sensor_chain=Chain(self.motor_chain)
        self.sensor_chain.setCoupling(self.sensor_locked_joints,self.sensor_finger_coupling.tolist())

        #pos fk
        self.pfk_solver=ChainFkSolverPos_recursive(self.motor_chain)
        self.cur_jpos_kdl=JntArray(self.motor_chain.getNrOfJoints())
        self.cur_frame_kdl=Frame()

        #vel fk
        self.vfk_solver=ChainFkSolverVel_recursive(self.motor_chain)
        self.cur_jvel_kdl=JntArrayVel(self.motor_chain.getNrOfJoints()) #TODO for the thumb we don't have joint velocity yet, it has to be calculated.
        self.cur_framevel_kdl=FrameVel()

        #vel ifk
        from numpy import finfo, double
        self.smallest=finfo(double).eps
        self.vik_solver=ChainIkSolverVel_wdls(self.motor_chain,self.smallest)
        #TODO: make set weights methods
        self.qdot_out_kdl=JntArray(self.motor_chain.getNrOfUnlockedJoints())
        self.tw=Twist()

        #jac solver
        self.jac_solver_sensors=ChainJntToJacSolver(self.sensor_chain)
        self.jac_solver_sensors
        self.jac_sensors=Jacobian(self.sensor_chain.getNrOfIndJoints())
        self.forces=array([0.0]*6)

        #finger calibration values
        #angle offsets in radians of the finger joints
        self.base_angle_offset=0.0
        self.proximal_angle_offset=0.0
        self.distal_angle_offset=0.0
        self.distal2_angle_offset=0.0

        #weight matrices for the statics inverse
        self.static_weight_left=diag([1.0, 1.0, 1.0, 0.0, 0.0, 0.0])
        self.static_weight_right=identity(self.sensor_chain.getNrOfIndJoints())
        self.static_weight_right=identity(self.sensor_chain.getNrOfUnlockedJoints())

    def get_cart_pos(self,segment_num=-1):
        if self.pfk_solver.JntToCart(self.cur_jpos_kdl,self.cur_frame_kdl,segment_num)<0:
            print "Error in cart pos calculation"

    def get_cart_vel(self):
        #TODO: not tested yet
#        for i,(angle,speed) in enumerate(zip(self.parent.cur_joint_pos,self.parent.cur_joint_vel)):
#            self.cur_jpos_kdl[i]=angle
#            self.cur_jvel_kdl.q[i]=angle
#            self.cur_jvel_kdl.qdot[i]=speed
        if self.vfk_solver.JntToCart(self.cur_jvel_kdl,self.cur_framevel_kdl)<0:
            print "Error in cart vel calculation"

    def calc_vik(self,tw_list):
        if len(tw_list)==6:
            for i in xrange(6):
                self.tw[i]=tw_list[i]
            if self.vik_solver.CartToJnt(self.cur_jpos_kdl,self.tw,self.qdot_out_kdl)<0:
                print "Error in velocity inverse kinematics"
                return(-1)
        else:
            print "Error wrong number of values"
            return(-1)
        return(0)

    def update_cur_jvel_kdl(self):
        j=0
        temp=concatenate((self.parent.cur_joint_vel,array([self.parent.cur_joint_vel[-1]])))
        for i,locked in enumerate(self.sensor_locked_joints):
            self.cur_jvel_kdl.q[i]=self.cur_jpos_kdl[i]
            if locked:
                self.cur_jvel_kdl.qdot[i]=temp[j]
                j+=1
            else:
                self.cur_jvel_kdl.qdot[i]=0.0
        

    def update_cur_jpos_kdl(self):
        j=0
        temp=concatenate((self.parent.cur_joint_pos,array([self.parent.cur_joint_pos[-1]])))
        for i,locked in enumerate(self.sensor_locked_joints):
            if locked:
                self.cur_jpos_kdl[i]=temp[j]
                j+=1
            else:
                self.cur_jpos_kdl[i]=0.0


    def get_forces(self):
        self.update_cur_jpos_kdl()
        #print "Test1"
        #print "Cur jpos kdl", [self.cur_jpos_kdl[i] for i in xrange(8)]
        if self.jac_solver_sensors.JntToJac(self.cur_jpos_kdl,self.jac_sensors,True)<0:
            print "Error in sensor jacobian calculation"
            return
        #print "Test2"

        jac_sensors_np=kdl_jac_to_numpy_array(self.jac_sensors)
#        print "pinv(jac.T)", pinv(jac_sensors_np.T)
#        print "cur torques", self.parent.cur_torques
        #print "Test3"
        try:
            self.forces=dot(weighted_pseudo_inverse(jac_sensors_np.T,self.static_weight_right,self.static_weight_left),self.parent.cur_torques)
        except Exception,e:
            print "Error during force calculation, force value not updated. Error:", e
Exemplo n.º 6
0
class DhChain:
    def __init__(self, config=[[0, 0, 0, 0]]):
        # Determine number of links

        # import code; code.interact(local=locals())
        # try:
        self._M = len(config['dh'])
        rospy.logdebug("Initializing dh chain with [%u] links", self._M)

        param_mat = config['dh']

        # self._length = param_mat.size       # The number of params needed to configure this chain
        # self._config = param_mat            # Mx4 matrix. Each row is a link, containing [theta, alpha, a, d]

        self._length = len(param_mat * 6)
        self._config = param_mat

        self._cov_dict = config['cov']
        self._gearing = config['gearing']
        # except:
        # self._cov_dict=config['cov']
        # self._M=len(self._cov_dict['joint_angles'])
        # param_mat = numpy.matrix([[0]*4]*self._M, float)
        # self._length = param_mat.size       # The number of params needed to configure this chain
        # self._config = param_mat            # Mx4 matrix. Each row is a link, containing [theta, alpha, a, d]
        # self._gearing=[0]*self._M

    def calc_free(self, free_config):
        # import code; code.interact(local=locals())
        # print len(free_config['dh'])
        # print self._M
        if free_config is None:
            free_config = {}
        if 'dh' not in free_config:
            free_config['dh']["xyzrpy"] = [[0] * 6] * self._M
        #else:
        #print free_config["dh"]
        #print [f for f in free_config["dh"]]
        #free_config['dh'] = [f["xyzrpy"] for f in free_config["dh"]]
        if 'gearing' not in free_config:
            free_config['gearing'] = [0] * self._M
        assert (len(free_config['dh']) == self._M)
        assert (len(free_config['gearing']) == self._M)

        # Flatten the config
        flat_config = []
        if isinstance(free_config["dh"][0], dict):
            for cfg in free_config["dh"]:
                flat_config.extend(cfg["xyzrpy"])
        else:
            flat_config = sum(free_config['dh'], [])

        assert (len(flat_config) == self._M * 6)

        flat_config = flat_config + free_config['gearing']

        # Convert int list into bool list
        flat_free = [x == 1 for x in flat_config]

        return flat_free

    def params_to_config(self, param_vec):
        assert (param_vec.shape == (self._M * 7, 1)
                )  # 6 pose params + 1 gearing per joint
        dh_param_vec = param_vec[0:(self._M * 6), 0]
        gearing_param_vec = param_vec[(self._M * 6):, 0]

        param_mat = reshape(dh_param_vec.T, (-1, 6))
        config_dict = {}
        config_dict['dh'] = self._config
        for sc, c in zip(config_dict["dh"], param_mat):
            sc["xyzrpy"] = c.tolist()[:][0]
        config_dict['gearing'] = (array(gearing_param_vec)[:, 0]).tolist()
        config_dict['cov'] = self._cov_dict
        return config_dict

    # Convert column vector of params into config
    def inflate(self, param_vec):
        param_mat = param_vec[:self._M * 6, :]
        config = reshape(param_mat, (-1, 6))
        for sc, c in zip(self._config, config):
            sc["xyzrpy"] = c.tolist()[:][0]
        gearing = array(param_vec[self._M * 6:, :])[:, 0].tolist()
        if len(gearing) is len(self._gearing):
            self._gearing = gearing

    # Return column vector of config
    def deflate(self):
        # param_vec = reshape(self._config, (-1,1))
        # param_vec = numpy.concatenate([param_vec, matrix(self._gearing).T])
        param_vec = []
        for joint in self._config:
            param_vec.extend(joint["xyzrpy"])
        param_vec.extend(self._gearing)
        param_vec = matrix(param_vec).T
        return param_vec

    # Returns # of params needed for inflation & deflation
    def get_length(self):
        return self._M * 7

    # Returns 4x4 numpy matrix of the pose of the tip of
    # the specified link num. Assumes the last link's tip
    # when link_num < 0
    def fk(self, chain_state, link_num=-2):
        if link_num < 0:
            link_num -= 1
        link_num += 1
        if link_num < 0:
            link_num = self._M

        pos_scaled = [
            cur_pos * cur_gearing for cur_pos, cur_gearing in zip(
                chain_state.actual.positions, self._gearing)
        ]

        #pos_scaled = JntArray(pos_scaled)
        jnt_states = JntArray(len(pos_scaled))
        for i, s in enumerate(pos_scaled):
            jnt_states[i] = s
        self.build_chain()
        F1 = Frame()

        assert (0 == self.fksolverpos.JntToCart(jnt_states, F1, link_num))

        out = []
        for i in range(3):
            line = []
            for j in range(3):
                line.append(F1.M[i, j])
            line.append(F1.p[i])
            out.append(line)
        out.append([0, 0, 0, 1])
        out = matrix(out)
        return out

    def build_chain(self):
        self.chain = Chain()
        for e in self._config:
            v = Vector(e["xyzrpy"][0], e["xyzrpy"][1], e["xyzrpy"][2])
            r = Rotation.RPY(e["xyzrpy"][3], e["xyzrpy"][4], e["xyzrpy"][5])
            j = Joint(e["name"], Joint.None)

            f = Frame(r, v)

            if e["type"] == "rotx":
                axis = Vector(1.0, 0.0, 0.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
            elif e["type"] == "roty":
                axis = Vector(0.0, 1.0, 0.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
            elif e["type"] == "rotz":
                axis = Vector(0.0, 0.0, 1.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
            elif e["type"] == "transx":
                axis = Vector(1.0, 0.0, 0.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
            elif e["type"] == "transy":
                axis = Vector(0.0, 1.0, 0.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
            elif e["type"] == "transz":
                axis = Vector(0.0, 0.0, 1.0)
                j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)

            self.chain.addSegment(
                Segment(e["name"] + "seg", j, f, RigidBodyInertia()))
        self.fksolverpos = ChainFkSolverPos_recursive(self.chain)