Esempio n. 1
0
    def coriolis(self, q, dq):
        c_kdl = kdl.JntArray(self.num_joints)

        self._dyn_kdl.JntToCoriolis(joint_list_to_kdl(q),
                                    joint_list_to_kdl(dq), c_kdl)
        return joint_kdl_to_list(c_kdl)
    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")
Esempio n. 3
0
    def __init__(self, T=20, weight=[1.0, 1.0], verbose=True):
        #initialize model
        self.chain = PyKDL.Chain()
        self.chain.addSegment(
            PyKDL.Segment(
                "Base", PyKDL.Joint(PyKDL.Joint.None),
                PyKDL.Frame(PyKDL.Vector(0.0, 0.0, 0.042)),
                PyKDL.RigidBodyInertia(
                    0.08659, PyKDL.Vector(0.00025, 0.0, -0.02420),
                    PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
        self.chain.addSegment(
            PyKDL.Segment(
                "Joint1", PyKDL.Joint(PyKDL.Joint.RotZ),
                PyKDL.Frame(PyKDL.Vector(0.0, -0.019, 0.028)),
                PyKDL.RigidBodyInertia(
                    0.00795, PyKDL.Vector(0.0, 0.019, -0.02025),
                    PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
        self.chain.addSegment(
            PyKDL.Segment(
                "Joint2", PyKDL.Joint(PyKDL.Joint.RotY),
                PyKDL.Frame(PyKDL.Vector(0.0, 0.019, 0.0405)),
                PyKDL.RigidBodyInertia(
                    0.09312, PyKDL.Vector(0.00000, -0.00057, -0.02731),
                    PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
        self.chain.addSegment(
            PyKDL.Segment(
                "Joint3", PyKDL.Joint(PyKDL.Joint.RotZ),
                PyKDL.Frame(PyKDL.Vector(0.024, -0.019, 0.064)),
                PyKDL.RigidBodyInertia(
                    0.19398, PyKDL.Vector(-0.02376, 0.01864, -0.02267),
                    PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
        self.chain.addSegment(
            PyKDL.Segment(
                "Joint4",
                PyKDL.Joint("minus_RotY", PyKDL.Vector(0, 0, 0),
                            PyKDL.Vector(0, -1, 0), PyKDL.Joint.RotAxis),
                PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.024)),
                PyKDL.RigidBodyInertia(
                    0.09824, PyKDL.Vector(-0.02099, 0.0, -0.01213),
                    PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
        self.chain.addSegment(
            PyKDL.Segment(
                "Joint5", PyKDL.Joint(PyKDL.Joint.RotX),
                PyKDL.Frame(PyKDL.Vector(0.0405, -0.019, 0.0)),
                PyKDL.RigidBodyInertia(
                    0.09312, PyKDL.Vector(-0.01319, 0.01843, 0.0),
                    PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
        self.chain.addSegment(
            PyKDL.Segment(
                "Joint6",
                PyKDL.Joint("minus_RotY", PyKDL.Vector(0, 0, 0),
                            PyKDL.Vector(0, -1, 0), PyKDL.Joint.RotAxis),
                PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.0)),
                PyKDL.RigidBodyInertia(
                    0.09824, PyKDL.Vector(-0.02099, 0.0, 0.01142),
                    PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))
        self.chain.addSegment(
            PyKDL.Segment(
                "Joint7", PyKDL.Joint(PyKDL.Joint.RotX),
                PyKDL.Frame(PyKDL.Vector(0.14103, 0.0, 0.0)),
                PyKDL.RigidBodyInertia(
                    0.26121, PyKDL.Vector(-0.09906, 0.00146, -0.00021),
                    PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0))))

        self.min_position_limit = [
            -160.0, -90.0, -160.0, -90.0, -160.0, -90.0, -160.0
        ]
        self.max_position_limit = [
            160.0, 90.0, 160.0, 90.0, 160.0, 90.0, 160.0
        ]
        self.min_joint_position_limit = PyKDL.JntArray(7)
        self.max_joint_position_limit = PyKDL.JntArray(7)
        for i in range(0, 7):
            self.min_joint_position_limit[
                i] = self.min_position_limit[i] * pi / 180
            self.max_joint_position_limit[
                i] = self.max_position_limit[i] * pi / 180

        self.fksolver = PyKDL.ChainFkSolverPos_recursive(self.chain)
        self.iksolver = PyKDL.ChainIkSolverVel_pinv(self.chain)
        self.iksolverpos = PyKDL.ChainIkSolverPos_NR_JL(
            self.chain, self.min_joint_position_limit,
            self.max_joint_position_limit, self.fksolver, self.iksolver, 100,
            1e-6)

        #parameter
        self.T = T
        self.weight_u = weight[0]
        self.weight_x = weight[1]
        self.verbose = verbose

        self.nj = self.chain.getNrOfJoints()
        self.q_init = np.zeros(self.nj)
        self.q_out = np.zeros(self.nj)

        ret, self.dest, self.q_out = self.generate_dest()
        self.fin_position = self.dest.p
        return
Esempio n. 4
0
    def compute_ff(self):
        stance = self.get_stance_legs()

        joint_pos = np.array(self.joint_positions).copy()

        p_ = []
        for i in range(4):
            frame = kdl.Frame()
            joints = kdl.JntArray(3)
            joints[0] = joint_pos[i + 0]
            joints[1] = joint_pos[i + 1]
            joints[2] = joint_pos[i + 2]
            self.fk_list[i].JntToCart(joints, frame)
            frame_p = np.array([frame.p[0], frame.p[1], frame.p[2]])
            foot_pos = frame_p
            p_.append(foot_pos)

        acc = np.zeros(3)

        constraints = []
        constraints_b = []
        equality_const = []
        equality_const_b = []
        theta = np.pi / 4
        F_normal = []
        num_stance = 0
        for i, s in enumerate(stance):
            if s:
                const = np.zeros(12)
                const[i * 3 + 0] = 0
                const[i * 3 + 1] = 0
                const[i * 3 + 2] = -1
                constraints.append(const)
                constraints_b.append(0)

                F_normal.append([0, 0, self.mass * 9.81])
                num_stance = num_stance + 1
            else:
                const = np.zeros(12)
                const[i * 3 + 0] = 1
                const[i * 3 + 1] = 0
                const[i * 3 + 2] = 0
                equality_const.append(const)
                equality_const_b.append(0)

                const = np.zeros(12)
                const[i * 3 + 0] = 0
                const[i * 3 + 1] = 1
                const[i * 3 + 2] = 0
                equality_const.append(const)
                equality_const_b.append(0)

                const = np.zeros(12)
                const[i * 3 + 0] = 0
                const[i * 3 + 1] = 0
                const[i * 3 + 2] = 1
                equality_const.append(const)
                equality_const_b.append(0)

                F_normal.append([0, 0, 0])

        F_normal = np.array(F_normal) / num_stance

        eye3 = np.eye(3)
        Atop = np.hstack((eye3, eye3, eye3, eye3))
        Abot = np.hstack((rm.skew_3d(p_[0]), rm.skew_3d(p_[1]),
                          rm.skew_3d(p_[2]), rm.skew_3d(p_[3])))
        A = np.vstack([Atop, Abot])

        g = np.array([0, 0, 9.8])
        bd_top = self.mass * g
        bd_top = np.reshape(bd_top, (3, 1))
        bd_bot = np.zeros(3)
        bd_bot = np.reshape(bd_bot, (3, 1))
        bd = np.vstack((bd_top, bd_bot))

        P = np.eye(12)
        q = np.zeros(12)

        G = np.array(constraints)
        h = np.array(constraints_b)

        if not equality_const == []:
            A = np.vstack((A, np.array(equality_const)))
            b = np.vstack((bd, np.array(equality_const_b)))[:, 0]
        else:
            A = A
            b = bd[:, 0]

        # print("qwer")
        # print(P)
        # print(q)
        # print(G)
        # print(h)
        # print(A)
        # print(b)

        F_opt = rm.quadprog_solve_qp(P, q, G=G, h=h, A=A, b=b)
        # print("f_opt")
        # print(F_opt)
        if np.abs(np.sum(F_opt)) < 1e-5:
            foot_forces = np.array(F_normal)
            # foot_forces = F_opt.reshape(4, 3)
            # print(foot_forces.shape)
        else:
            foot_forces = F_opt
            # print(foot_forces.shape)
            # print("ihi")
        # print(foot_forces)
        torques = self.force_to_torques(foot_forces, joint_pos)
        # print(torques)
        return torques
def limbPose(kdl_tree, base_link, limb_interface, limb='right'):
    tip_link = limb + '_gripper'
    tip_frame = PyKDL.Frame()
    arm_chain = kdl_tree.getChain(base_link, tip_link)

    # Baxter Interface Limb Instances
    #limb_interface = baxter_interface.Limb(limb)
    joint_names = limb_interface.joint_names()
    num_jnts = len(joint_names)

    if limb == 'right':
        limb_link = [
            'base', 'torso', 'right_arm_mount', 'right_upper_shoulder',
            'right_lower_shoulder', 'right_upper_elbow', 'right_lower_elbow',
            'right_upper_forearm', 'right_lower_forearm', 'right_wrist',
            'right_hand', 'right_gripper_base', 'right_gripper'
        ]
    else:
        limb_link = [
            'base', 'torso', 'left_arm_mount', 'left_upper_shoulder',
            'left_lower_shoulder', 'left_upper_elbow', 'left_lower_elbow',
            'left_upper_forearm', 'left_lower_forearm', 'left_wrist',
            'left_hand', 'left_gripper_base', 'left_gripper'
        ]
    limb_frame = []
    limb_chain = []
    limb_pose = []
    limb_fk = []

    for idx in xrange(arm_chain.getNrOfSegments()):
        linkname = limb_link[idx]
        limb_frame.append(PyKDL.Frame())
        limb_chain.append(kdl_tree.getChain(base_link, linkname))
        limb_fk.append(
            PyKDL.ChainFkSolverPos_recursive(
                kdl_tree.getChain(base_link, linkname)))

    # get the joint positions
    cur_type_values = limb_interface.joint_angles()
    while len(cur_type_values) != 7:
        print "Joint angles error!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
        cur_type_values = limb_interface.joint_angles()
    kdl_array = PyKDL.JntArray(num_jnts)
    for idx, name in enumerate(joint_names):
        kdl_array[idx] = cur_type_values[name]

    limb_joint = [
        PyKDL.JntArray(1),
        PyKDL.JntArray(2),
        PyKDL.JntArray(3),
        PyKDL.JntArray(4),
        PyKDL.JntArray(5),
        PyKDL.JntArray(6),
        PyKDL.JntArray(7)
    ]
    for i in range(7):
        for j in range(i + 1):
            limb_joint[i][j] = kdl_array[j]

    for i in range(arm_chain.getNrOfSegments()):
        joint_array = limb_joint[limb_chain[i].getNrOfJoints() - 1]
        limb_fk[i].JntToCart(joint_array, limb_frame[i])
        pos = limb_frame[i].p
        rot = PyKDL.Rotation(limb_frame[i].M)
        rot = rot.GetQuaternion()
        limb_pose.append([pos[0], pos[1], pos[2]])

    return np.asarray(limb_pose), kdl_array
Esempio n. 6
0
 def convert_joint_angles_array_to_kdl_array(self, joint_angles_array):
     num_joints = len(joint_angles_array)
     kdl_array = PyKDL.JntArray(num_joints)
     for i in range(num_joints):
         kdl_array[i] = joint_angles_array[i]
     return kdl_array
Esempio n. 7
0
def forward_kinematics(data):
    chain = PyKDL.Chain()
    joint_movement = [PyKDL.Joint.RotZ, PyKDL.Joint.RotZ,PyKDL.Joint.TransZ]  
    n_joints = len(params.keys())
    for i in range(n_joints):
        _, d, alpha, th = params['i'+str(i+1)]
        try:
            a, _, _, _ = params['i'+str(i+2)]
        except:
            a, _ = 0, 0
        alpha, a, d, th = float(alpha), float(a), float(d), float(th)
        frame = PyKDL.Frame()
        joint = PyKDL.Joint(joint_movement[i])
        frame = frame.DH(a, alpha, d, th)
        segment = PyKDL.Segment(joint, frame)
        chain.addSegment(segment)

    joints = PyKDL.JntArray(n_joints)
    for i in range(n_joints):
        min_joint, max_joint = scope["joint"+str(i+1)]
        if min_joint <= data.position[i] <= max_joint:
            if i == 2:
                joints[i] = -data.position[i]
            else:
                joints[i] = data.position[i]
        else:
            rospy.logwarn("Wrong joint value")
            return
    fk=PyKDL.ChainFkSolverPos_recursive(chain)
    finalFrame=PyKDL.Frame()
    fk.JntToCart(joints,finalFrame)
    quaterions = finalFrame.M.GetQuaternion()

    pose = PoseStamped()
    pose.header.frame_id = 'base_link'
    pose.header.stamp = rospy.Time.now()
    pose.pose.position.x = finalFrame.p[0]
    pose.pose.position.y = finalFrame.p[1] 
    pose.pose.position.z = finalFrame.p[2]
    pose.pose.orientation.x = quaterions[3]
    pose.pose.orientation.y = quaterions[2]
    pose.pose.orientation.z = quaterions[1]
    pose.pose.orientation.w = quaterions[0]
    pub.publish(pose)

    marker = Marker()
    marker.header.frame_id = 'base_link'
    marker.type = marker.CUBE
    marker.action = marker.ADD
    marker.pose.orientation.w = 1

    time = rospy.Duration()
    marker.lifetime = time
    marker.scale.x = 0.09
    marker.scale.y = 0.09
    marker.scale.z = 0.09
    marker.pose.position.x = finalFrame.p[0];
    marker.pose.position.y = finalFrame.p[1];
    marker.pose.position.z = finalFrame.p[2];
    marker.pose.orientation.x = quaterions[3];
    marker.pose.orientation.y = quaterions[2];
    marker.pose.orientation.z = quaterions[1];
    marker.pose.orientation.w = quaterions[0];
    marker.color.a = 0.7
    marker.color.r = 0.2
    marker.color.g = 0.8
    marker.color.b = 0.6
    marker_pub.publish(marker)
Esempio n. 8
0
frm5 = kdl.Frame(kdl.Rotation.RotX(np.pi / 2), kdl.Vector(0, 0, 0))
frm6 = kdl.Frame(kdl.Rotation.RotX(0), kdl.Vector(-168, 0, 0))

frms.append(frm1)
frms.append(frm2)
frms.append(frm3)
frms.append(frm4)
frms.append(frm5)
frms.append(frm6)

rbt = kdl.Chain()  # 建立机器人对象

link = []
for i in range(6):
    link.append(kdl.Segment(jnts[i], frms[i]))
    rbt.addSegment(link[i])

fk = kdl.ChainFkSolverPos_recursive(rbt)

p = kdl.Frame()
q = kdl.JntArray(6)
q[0] = 0
q[1] = 0
q[2] = 0
q[3] = 0
q[4] = 0
q[5] = 0

fk.JntToCart(q, p)

print(p)
        print("Get urdf model from parameter server")
        model = URDF.from_parameter_server()

        #print model

        print("Loading kdl tree using urdf from parameter server")
        ret, kdltree = urdf.treeFromUrdfModel(model, quiet=False)

        # Get the whole kinematic chain from kdl tree
        chain = kdltree.getChain(base_link, end_link)

        # Initialize kdl solver for jacobian calculation
        solver = kdl.ChainJntToJacSolver(chain)

        jnt_array = kdl.JntArray(chain.getNrOfSegments())

        # Get joints from the chain
        #for i in range(chain.getNrOfSegments()):
        #    print chain.getSegment(i).getJoint()

        jac = kdl.Jacobian(chain.getNrOfSegments())
        #        print(dir(kdl))

        my_arr = np.zeros((6, 7))

        if (solver.JntToJac(jnt_array, jac) == 0):
            rospy.loginfo("Sucessfully calculated jacobian")
            print jac[0, 3]

            for i in range(0, 6):
Esempio n. 10
0
    def __init__(self):

        filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/mi_hexapodo_exp.urdf'

        angulo_avance = +0.20 #rad
        altura_pata = +0.03 #cm
        # angulo_avance = 0  # +0.40 #rad
        # altura_pata = 0  # -0.04 #cm

        robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)

        (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
        cadena_RR = tree.getChain("base_link", "tarsus_RR")
        cadena_RM = tree.getChain("base_link", "tarsus_RM")
        cadena_RF = tree.getChain("base_link", "tarsus_RF")
        cadena_LR = tree.getChain("base_link", "tarsus_LR")
        cadena_LM = tree.getChain("base_link", "tarsus_LM")
        cadena_LF = tree.getChain("base_link", "tarsus_LF")

        print cadena_RR.getNrOfSegments()
        fksolver_RR = PyKDL.ChainFkSolverPos_recursive(cadena_RR)
        fksolver_RM= PyKDL.ChainFkSolverPos_recursive(cadena_RM)
        fksolver_RF = PyKDL.ChainFkSolverPos_recursive(cadena_RF)
        fksolver_LR = PyKDL.ChainFkSolverPos_recursive(cadena_LR)
        fksolver_LM = PyKDL.ChainFkSolverPos_recursive(cadena_LM)
        fksolver_LF = PyKDL.ChainFkSolverPos_recursive(cadena_LF)

        vik_RR = PyKDL.ChainIkSolverVel_pinv(cadena_RR)
        ik_RR = PyKDL.ChainIkSolverPos_NR(cadena_RR, fksolver_RR, vik_RR)

        vik_RM = PyKDL.ChainIkSolverVel_pinv(cadena_RM)
        ik_RM = PyKDL.ChainIkSolverPos_NR(cadena_RM, fksolver_RM, vik_RM)

        vik_RF = PyKDL.ChainIkSolverVel_pinv(cadena_RF)
        ik_RF = PyKDL.ChainIkSolverPos_NR(cadena_RF, fksolver_RF, vik_RF)

        vik_LR = PyKDL.ChainIkSolverVel_pinv(cadena_LR)
        ik_LR = PyKDL.ChainIkSolverPos_NR(cadena_LR, fksolver_LR, vik_LR)

        vik_LM = PyKDL.ChainIkSolverVel_pinv(cadena_LM)
        ik_LM = PyKDL.ChainIkSolverPos_NR(cadena_LM, fksolver_LM, vik_LM)

        vik_LF = PyKDL.ChainIkSolverVel_pinv(cadena_LF)
        ik_LF = PyKDL.ChainIkSolverPos_NR(cadena_LF, fksolver_LF, vik_LF)

        nj_izq = cadena_RR.getNrOfJoints()
        posicionInicial_R = PyKDL.JntArray(nj_izq)
        posicionInicial_R[0] = 0
        posicionInicial_R[1] = 0
        posicionInicial_R[2] = 0
        posicionInicial_R[3] = 0

        nj_izq = cadena_LR.getNrOfJoints()
        posicionInicial_L = PyKDL.JntArray(nj_izq)
        posicionInicial_L[0] = 0
        posicionInicial_L[1] = 0
        posicionInicial_L[2] = 0
        posicionInicial_L[3] = 0

        print "Forward kinematics"
        finalFrame_R = PyKDL.Frame()
        finalFrame_L = PyKDL.Frame()

        rospy.init_node('trajectory_demo')

        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)

        # Set to False to wait for arm to finish before moving head
        sync = rospy.get_param('~sync', True)

        # Which joints define the arm?
        piernas_joints_RR = ['coxa_joint_RR', 'femur_joint_RR','tibia_joint_RR', 'tarsus_joint_RR']
        piernas_joints_LM = ['coxa_joint_LM', 'femur_joint_LM','tibia_joint_LM', 'tarsus_joint_LM']
        piernas_joints_RF = ['coxa_joint_RF', 'femur_joint_RF','tibia_joint_RF', 'tarsus_joint_RF']

        piernas_joints_LR = ['coxa_joint_LR', 'femur_joint_LR', 'tibia_joint_LR', 'tarsus_joint_LR']
        piernas_joints_RM = ['coxa_joint_RM', 'femur_joint_RM', 'tibia_joint_RM', 'tarsus_joint_RM']
        piernas_joints_LF = ['coxa_joint_LF', 'femur_joint_LF', 'tibia_joint_LF', 'tarsus_joint_LF']

        rr_goal0 = [0.0, 0.0, 0.0, 0.0]
        lm_goal0 = [0.0, 0.0, 0.0, 0.0]
        rf_goal0 = [0.0, 0.0, 0.0, 0.0]
        rr_goal1 = [0.0, 0.0, 0.0, 0.0]
        lm_goal1 = [0.0, 0.0, 0.0, 0.0]
        rf_goal1 = [0.0, 0.0, 0.0, 0.0]
        lr_goal0 = [0.0, 0.0, 0.0, 0.0]
        rm_goal0 = [0.0, 0.0, 0.0, 0.0]
        lf_goal0 = [0.0, 0.0, 0.0, 0.0]
        lr_goal1 = [0.0, 0.0, 0.0, 0.0]
        rm_goal1 = [0.0, 0.0, 0.0, 0.0]
        lf_goal1 = [0.0, 0.0, 0.0, 0.0]

        #11111111111111111111111111111111111111111111
        print "Inverse Kinematics"
        fksolver_RR.JntToCart(posicionInicial_R, finalFrame_R)
        q_init_RR = posicionInicial_R  # initial angles
        desiredFrameRR = finalFrame_R
        desiredFrameRR.p[0] = finalFrame_R.p[0]
        desiredFrameRR.p[1] = finalFrame_R.p[1]
        desiredFrameRR.p[2] = finalFrame_R.p[2]+altura_pata
        print "Desired Position: ", desiredFrameRR.p
        q_out_RR = PyKDL.JntArray(cadena_RR.getNrOfJoints())
        ik_RR.CartToJnt(q_init_RR, desiredFrameRR, q_out_RR)
        print "Output angles in rads: ", q_out_RR

        rr_goal0[0] = q_out_RR[0]+angulo_avance
        rr_goal0[1] = q_out_RR[1]
        rr_goal0[2] = q_out_RR[2]
        rr_goal0[3] = q_out_RR[3]

        print "Inverse Kinematics"
        fksolver_LM.JntToCart(posicionInicial_L, finalFrame_L)
        q_init_LM = posicionInicial_L  # initial angles
        desiredFrameLM = finalFrame_L
        desiredFrameLM.p[0] = finalFrame_L.p[0]
        desiredFrameLM.p[1] = finalFrame_L.p[1]
        desiredFrameLM.p[2] = finalFrame_L.p[2] +altura_pata
        print "Desired Position: ", desiredFrameLM.p
        q_out_LM = PyKDL.JntArray(cadena_LM.getNrOfJoints())
        ik_LM.CartToJnt(q_init_LM, desiredFrameLM, q_out_LM)
        print "Output angles in rads: ", q_out_LM

        lm_goal0[0] = q_out_LM[0]-angulo_avance
        lm_goal0[1] = q_out_LM[1]
        lm_goal0[2] = q_out_LM[2]
        lm_goal0[3] = q_out_LM[3]

        print "Inverse Kinematics"
        fksolver_RF.JntToCart(posicionInicial_R, finalFrame_R)
        q_init_RF = posicionInicial_R  # initial angles
        desiredFrameRF = finalFrame_R
        desiredFrameRF.p[0] = finalFrame_R.p[0]
        desiredFrameRF.p[1] = finalFrame_R.p[1]
        desiredFrameRF.p[2] = finalFrame_R.p[2] + altura_pata
        print "Desired Position: ", desiredFrameRF.p
        q_out_RF = PyKDL.JntArray(cadena_RF.getNrOfJoints())
        ik_RF.CartToJnt(q_init_RF, desiredFrameRF, q_out_RF)
        print "Output angles in rads: ", q_out_RF

        rf_goal0[0] = q_out_RF[0]+angulo_avance
        rf_goal0[1] = q_out_RF[1]
        rf_goal0[2] = q_out_RF[2]
        rf_goal0[3] = q_out_RF[3]

        # 22222222222222222222222222222222222222222222
        RR_actual = PyKDL.JntArray(nj_izq)
        RR_actual[0] = rr_goal0[0]
        RR_actual[1] = rr_goal0[1]
        RR_actual[2] = rr_goal0[2]
        RR_actual[3] = rr_goal0[3]
        print "Inverse Kinematics"
        fksolver_RR.JntToCart(RR_actual, finalFrame_R)
        q_init_RR = RR_actual  # initial angles
        desiredFrameRR = finalFrame_R
        desiredFrameRR.p[0] = finalFrame_R.p[0]
        desiredFrameRR.p[1] = finalFrame_R.p[1]
        desiredFrameRR.p[2] = finalFrame_R.p[2] - altura_pata
        print "Desired Position: ", desiredFrameRR.p
        q_out_RR = PyKDL.JntArray(cadena_RR.getNrOfJoints())
        ik_RR.CartToJnt(q_init_RR, desiredFrameRR, q_out_RR)
        print "Output angles in rads: ", q_out_RR

        rr_goal1[0] = q_out_RR[0]
        rr_goal1[1] = q_out_RR[1]
        rr_goal1[2] = q_out_RR[2]
        rr_goal1[3] = q_out_RR[3]

        rr_goal2 = rr_goal1[:]
        rr_goal2[0] = -angulo_avance

        LM_actual = PyKDL.JntArray(nj_izq)
        LM_actual[0] = lm_goal0[0]
        LM_actual[1] = lm_goal0[1]
        LM_actual[2] = lm_goal0[2]
        LM_actual[3] = lm_goal0[3]
        print "Inverse Kinematics"
        fksolver_LM.JntToCart(LM_actual, finalFrame_L)
        q_init_LM = LM_actual  # initial angles
        desiredFrameLM = finalFrame_L
        desiredFrameLM.p[0] = finalFrame_L.p[0]
        desiredFrameLM.p[1] = finalFrame_L.p[1]
        desiredFrameLM.p[2] = finalFrame_L.p[2] - altura_pata
        print "Desired Position: ", desiredFrameLM.p
        q_out_LM = PyKDL.JntArray(cadena_LM.getNrOfJoints())
        ik_LM.CartToJnt(q_init_LM, desiredFrameLM, q_out_LM)
        print "Output angles in rads: ", q_out_LM

        lm_goal1[0] = q_out_LM[0]
        lm_goal1[1] = q_out_LM[1]
        lm_goal1[2] = q_out_LM[2]
        lm_goal1[3] = q_out_LM[3]

        lm_goal2 = lm_goal1[:]
        lm_goal2[0] = angulo_avance

        RF_actual = PyKDL.JntArray(nj_izq)
        RF_actual[0] = rf_goal0[0]
        RF_actual[1] = rf_goal0[1]
        RF_actual[2] = rf_goal0[2]
        RF_actual[3] = rf_goal0[3]
        print "Inverse Kinematics"
        fksolver_RF.JntToCart(RF_actual, finalFrame_R)
        q_init_RF = RF_actual  # initial angles
        desiredFrameRF = finalFrame_R
        desiredFrameRF.p[0] = finalFrame_R.p[0]
        desiredFrameRF.p[1] = finalFrame_R.p[1]
        desiredFrameRF.p[2] = finalFrame_R.p[2]- altura_pata
        print "Desired Position: ", desiredFrameRF.p
        q_out_RF = PyKDL.JntArray(cadena_RF.getNrOfJoints())
        ik_RF.CartToJnt(q_init_RF, desiredFrameRF, q_out_RF)
        print "Output angles in rads: ", q_out_RF

        rf_goal1[0] = q_out_RF[0]
        rf_goal1[1] = q_out_RF[1]
        rf_goal1[2] = q_out_RF[2]
        rf_goal1[3] = q_out_RF[3]

        rf_goal2 = rf_goal1[:]
        rf_goal2[0] = -angulo_avance

        # 11111111111111111111111111111111111111111111
        print "Inverse Kinematics"
        fksolver_LR.JntToCart(posicionInicial_L, finalFrame_L)
        q_init_LR = posicionInicial_L  # initial angles
        desiredFrameLR = finalFrame_L
        desiredFrameLR.p[0] = finalFrame_L.p[0]
        desiredFrameLR.p[1] = finalFrame_L.p[1]
        desiredFrameLR.p[2] = finalFrame_L.p[2]  # + altura_pata
        print "Desired Position: ", desiredFrameLR.p
        q_out_LR = PyKDL.JntArray(cadena_LR.getNrOfJoints())
        ik_LR.CartToJnt(q_init_LR, desiredFrameLR, q_out_LR)
        print "Output angles in rads: ", q_out_LR

        lr_goal0[0] = q_out_LR[0] + angulo_avance
        lr_goal0[1] = q_out_LR[1]
        lr_goal0[2] = q_out_LR[2]
        lr_goal0[3] = q_out_LR[3]

        print "Inverse Kinematics"
        fksolver_RM.JntToCart(posicionInicial_R, finalFrame_R)
        q_init_RM = posicionInicial_R  # initial angles
        desiredFrameRM = finalFrame_R
        desiredFrameRM.p[0] = finalFrame_R.p[0]
        desiredFrameRM.p[1] = finalFrame_R.p[1]
        desiredFrameRM.p[2] = finalFrame_R.p[2]  # + altura_pata
        print "Desired Position: ", desiredFrameRM.p
        q_out_RM = PyKDL.JntArray(cadena_RM.getNrOfJoints())
        ik_RM.CartToJnt(q_init_RM, desiredFrameRM, q_out_RM)
        print "Output angles in rads: ", q_out_RM

        rm_goal0[0] = q_out_RM[0] - angulo_avance
        rm_goal0[1] = q_out_RM[1]
        rm_goal0[2] = q_out_RM[2]
        rm_goal0[3] = q_out_RM[3]

        print "Inverse Kinematics"
        fksolver_LF.JntToCart(posicionInicial_L, finalFrame_L)
        q_init_LF = posicionInicial_L  # initial angles
        desiredFrameLF = finalFrame_L
        desiredFrameLF.p[0] = finalFrame_L.p[0]
        desiredFrameLF.p[1] = finalFrame_L.p[1]
        desiredFrameLF.p[2] = finalFrame_L.p[2]  # + altura_pata
        print "Desired Position: ", desiredFrameLF.p
        q_out_LF = PyKDL.JntArray(cadena_LF.getNrOfJoints())
        ik_LF.CartToJnt(q_init_LF, desiredFrameLF, q_out_LF)
        print "Output angles in rads: ", q_out_LF

        lf_goal0[0] = q_out_LF[0] + angulo_avance
        lf_goal0[1] = q_out_LF[1]
        lf_goal0[2] = q_out_LF[2]
        lf_goal0[3] = q_out_LF[3]

        # 2222222222222222222222222222222222222222222222

        LR_actual = PyKDL.JntArray(nj_izq)
        LR_actual[0] = lr_goal0[0]
        LR_actual[1] = lr_goal0[1]
        LR_actual[2] = lr_goal0[2]
        LR_actual[3] = lr_goal0[3]
        print "Inverse Kinematics"
        fksolver_LR.JntToCart(LR_actual, finalFrame_L)
        q_init_LR = LR_actual  # initial angles
        print "Inverse Kinematics"
        desiredFrameLR = finalFrame_L
        desiredFrameLR.p[0] = finalFrame_L.p[0]
        desiredFrameLR.p[1] = finalFrame_L.p[1]
        desiredFrameLR.p[2] = finalFrame_L.p[2]   + altura_pata
        print "Desired Position: ", desiredFrameLR.p
        q_out_LR = PyKDL.JntArray(cadena_LR.getNrOfJoints())
        ik_LR.CartToJnt(q_init_LR, desiredFrameLR, q_out_LR)
        print "Output angles in rads: ", q_out_LR

        lr_goal1[0] = q_out_LR[0] #- angulo_avance
        lr_goal1[1] = q_out_LR[1]
        lr_goal1[2] = q_out_LR[2]
        lr_goal1[3] = q_out_LR[3]

        RM_actual = PyKDL.JntArray(nj_izq)
        RM_actual[0] = rm_goal0[0]
        RM_actual[1] = rm_goal0[1]
        RM_actual[2] = rm_goal0[2]
        RM_actual[3] = rm_goal0[3]
        print "Inverse Kinematics"
        fksolver_RM.JntToCart(RM_actual, finalFrame_R)
        q_init_RM = RM_actual  # initial angles
        desiredFrameRM = finalFrame_R
        desiredFrameRM.p[0] = finalFrame_R.p[0]
        desiredFrameRM.p[1] = finalFrame_R.p[1]
        desiredFrameRM.p[2] = finalFrame_R.p[2]   + altura_pata
        print "Desired Position: ", desiredFrameRM.p
        q_out_RM = PyKDL.JntArray(cadena_RM.getNrOfJoints())
        ik_RM.CartToJnt(q_init_RM, desiredFrameRM, q_out_RM)
        print "Output angles in rads: ", q_out_RM

        rm_goal1[0] = q_out_RM[0] #- angulo_avance
        rm_goal1[1] = q_out_RM[1]
        rm_goal1[2] = q_out_RM[2]
        rm_goal1[3] = q_out_RM[3]

        LF_actual = PyKDL.JntArray(nj_izq)
        LF_actual[0] = lf_goal0[0]
        LF_actual[1] = lf_goal0[1]
        LF_actual[2] = lf_goal0[2]
        LF_actual[3] = lf_goal0[3]
        print "Inverse Kinematics"
        fksolver_LF.JntToCart(LF_actual, finalFrame_L)
        q_init_LF = LF_actual  # initial angles
        desiredFrameLF = finalFrame_L
        desiredFrameLF.p[0] = finalFrame_L.p[0]
        desiredFrameLF.p[1] = finalFrame_L.p[1]
        desiredFrameLF.p[2] = finalFrame_L.p[2]   + altura_pata
        print "Desired Position: ", desiredFrameLF.p
        q_out_LF = PyKDL.JntArray(cadena_LF.getNrOfJoints())
        ik_LF.CartToJnt(q_init_LF, desiredFrameLF, q_out_LF)
        print "Output angles in rads: ", q_out_LF

        lf_goal1[0] = q_out_LF[0] #- angulo_avance
        lf_goal1[1] = q_out_LF[1]
        lf_goal1[2] = q_out_LF[2]
        lf_goal1[3] = q_out_LF[3]

        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')

        rr_client = actionlib.SimpleActionClient('/hexapodo/pata_rr/follow_joint_trajectory',
                                                 FollowJointTrajectoryAction)

        rr_client.wait_for_server()

        lm_client = actionlib.SimpleActionClient('/hexapodo/pata_lm/follow_joint_trajectory',
                                                 FollowJointTrajectoryAction)
        lm_client.wait_for_server()

        rf_client = actionlib.SimpleActionClient('/hexapodo/pata_rf/follow_joint_trajectory',
                                                 FollowJointTrajectoryAction)
        rf_client.wait_for_server()

        lr_client = actionlib.SimpleActionClient('/hexapodo/pata_lr/follow_joint_trajectory',
                                                 FollowJointTrajectoryAction)
        lr_client.wait_for_server()

        rm_client = actionlib.SimpleActionClient('/hexapodo/pata_rm/follow_joint_trajectory',
                                                 FollowJointTrajectoryAction)
        rm_client.wait_for_server()

        lf_client = actionlib.SimpleActionClient('/hexapodo/pata_lf/follow_joint_trajectory',
                                                 FollowJointTrajectoryAction)
        lf_client.wait_for_server()

        rospy.loginfo('...connected.')

        # Create a single-point arm trajectory with the piernas_goal as the end-point
        rr_trajectory1 = JointTrajectory()
        rr_trajectory1.joint_names = piernas_joints_RR
        rr_trajectory1.points.append(JointTrajectoryPoint())
        rr_trajectory1.points[0].positions = rr_goal0
        rr_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RR]
        rr_trajectory1.points[0].accelerations = [0.0 for i in piernas_joints_RR]
        rr_trajectory1.points[0].time_from_start = rospy.Duration(0.25)
        rr_trajectory1.points.append(JointTrajectoryPoint())
        rr_trajectory1.points[1].positions = rr_goal1
        rr_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RR]
        rr_trajectory1.points[1].accelerations = [0.0 for i in piernas_joints_RR]
        rr_trajectory1.points[1].time_from_start = rospy.Duration(0.5)
        rr_trajectory1.points.append(JointTrajectoryPoint())
        rr_trajectory1.points[2].positions = rr_goal2
        rr_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RR]
        rr_trajectory1.points[2].accelerations = [0.0 for i in piernas_joints_RR]
        rr_trajectory1.points[2].time_from_start = rospy.Duration(0.75)
        rr_trajectory1.points.append(JointTrajectoryPoint())
        rr_trajectory1.points[3].positions = lr_goal0
        rr_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RR]
        rr_trajectory1.points[3].accelerations = [0.0 for i in piernas_joints_RR]
        rr_trajectory1.points[3].time_from_start = rospy.Duration(1.0)
        #'''
        lm_trajectory1 = JointTrajectory()
        lm_trajectory1.joint_names = piernas_joints_LM
        lm_trajectory1.points.append(JointTrajectoryPoint())
        lm_trajectory1.points[0].positions = lm_goal0#[0,0,0,0]
        lm_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_LM]
        lm_trajectory1.points[0].accelerations = [0.0 for i in piernas_joints_LM]
        lm_trajectory1.points[0].time_from_start = rospy.Duration(0.25)
        lm_trajectory1.points.append(JointTrajectoryPoint())
        lm_trajectory1.points[1].positions = lm_goal1  # [0,0,0,0]
        lm_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_LM]
        lm_trajectory1.points[1].accelerations = [0.0 for i in piernas_joints_LM]
        lm_trajectory1.points[1].time_from_start = rospy.Duration(0.5)
        lm_trajectory1.points.append(JointTrajectoryPoint())
        lm_trajectory1.points[2].positions = lm_goal2 # [0,0,0,0]
        lm_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_LM]
        lm_trajectory1.points[2].accelerations = [0.0 for i in piernas_joints_LM]
        lm_trajectory1.points[2].time_from_start = rospy.Duration(0.75)
        lm_trajectory1.points.append(JointTrajectoryPoint())
        lm_trajectory1.points[3].positions = rm_goal0  # [0,0,0,0]
        lm_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_LM]
        lm_trajectory1.points[3].accelerations = [0.0 for i in piernas_joints_LM]
        lm_trajectory1.points[3].time_from_start = rospy.Duration(1.0)
        # '''
        rf_trajectory1 = JointTrajectory()
        rf_trajectory1.joint_names = piernas_joints_RF
        rf_trajectory1.points.append(JointTrajectoryPoint())
        rf_trajectory1.points[0].positions = rf_goal0  # [0,0,0,0]
        rf_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RF]
        rf_trajectory1.points[0].accelerations = [0.0 for i in piernas_joints_RF]
        rf_trajectory1.points[0].time_from_start = rospy.Duration(0.25)
        rf_trajectory1.points.append(JointTrajectoryPoint())
        rf_trajectory1.points[1].positions = rf_goal1  # [0,0,0,0]
        rf_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RF]
        rf_trajectory1.points[1].accelerations = [0.0 for i in piernas_joints_RF]
        rf_trajectory1.points[1].time_from_start = rospy.Duration(0.5)
        rf_trajectory1.points.append(JointTrajectoryPoint())
        rf_trajectory1.points[2].positions = rf_goal2  # [0,0,0,0]
        rf_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RF]
        rf_trajectory1.points[2].accelerations = [0.0 for i in piernas_joints_RF]
        rf_trajectory1.points[2].time_from_start = rospy.Duration(0.75)
        rf_trajectory1.points.append(JointTrajectoryPoint())
        rf_trajectory1.points[3].positions = lf_goal0  # [0,0,0,0]
        rf_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RF]
        rf_trajectory1.points[3].accelerations = [0.0 for i in piernas_joints_RF]
        rf_trajectory1.points[3].time_from_start = rospy.Duration(1.0)
        # '''
        lr_trajectory1 = JointTrajectory()
        lr_trajectory1.joint_names = piernas_joints_LR
        lr_trajectory1.points.append(JointTrajectoryPoint())
        lr_trajectory1.points[0].positions = lr_goal0#lr_goal0
        lr_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RR]
        lr_trajectory1.points[0].accelerations = [0.0 for i in piernas_joints_RR]
        lr_trajectory1.points[0].time_from_start = rospy.Duration(0.25)
        lr_trajectory1.points.append(JointTrajectoryPoint())
        lr_trajectory1.points[1].positions = lr_goal1
        lr_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RR]
        lr_trajectory1.points[1].accelerations = [0.0 for i in piernas_joints_RR]
        lr_trajectory1.points[1].time_from_start = rospy.Duration(0.5)
        lr_trajectory1.points.append(JointTrajectoryPoint())
        lr_trajectory1.points[2].positions = rr_goal2
        lr_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RR]
        lr_trajectory1.points[2].accelerations = [0.0 for i in piernas_joints_RR]
        lr_trajectory1.points[2].time_from_start = rospy.Duration(0.75)
        lr_trajectory1.points.append(JointTrajectoryPoint())
        lr_trajectory1.points[3].positions = rr_goal0  # lr_goal0
        lr_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RR]
        lr_trajectory1.points[3].accelerations = [0.0 for i in piernas_joints_RR]
        lr_trajectory1.points[3].time_from_start = rospy.Duration(1.0)
        # '''
        rm_trajectory1 = JointTrajectory()
        rm_trajectory1.joint_names = piernas_joints_RM
        rm_trajectory1.points.append(JointTrajectoryPoint())
        rm_trajectory1.points[0].positions = rm_goal0#rm_goal0  # [0,0,0,0]
        rm_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_LM]
        rm_trajectory1.points[0].accelerations = [0.0 for i in piernas_joints_LM]
        rm_trajectory1.points[0].time_from_start = rospy.Duration(0.25)
        rm_trajectory1.points.append(JointTrajectoryPoint())
        rm_trajectory1.points[1].positions = rm_goal1  # [0,0,0,0]
        rm_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_LM]
        rm_trajectory1.points[1].accelerations = [0.0 for i in piernas_joints_LM]
        rm_trajectory1.points[1].time_from_start = rospy.Duration(0.5)
        rm_trajectory1.points.append(JointTrajectoryPoint())
        rm_trajectory1.points[2].positions = lm_goal2  # [0,0,0,0]
        rm_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_LM]
        rm_trajectory1.points[2].accelerations = [0.0 for i in piernas_joints_LM]
        rm_trajectory1.points[2].time_from_start = rospy.Duration(0.75)
        rm_trajectory1.points.append(JointTrajectoryPoint())
        rm_trajectory1.points[3].positions = lm_goal0  # rm_goal0  # [0,0,0,0]
        rm_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_LM]
        rm_trajectory1.points[3].accelerations = [0.0 for i in piernas_joints_LM]
        rm_trajectory1.points[3].time_from_start = rospy.Duration(1.0)
        # '''
        lf_trajectory1 = JointTrajectory()
        lf_trajectory1.joint_names = piernas_joints_LF
        lf_trajectory1.points.append(JointTrajectoryPoint())
        lf_trajectory1.points[0].positions = lf_goal0#lf_goal0  # [0,0,0,0]
        lf_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RF]
        lf_trajectory1.points[0].accelerations = [0.0 for i in piernas_joints_RF]
        lf_trajectory1.points[0].time_from_start = rospy.Duration(0.25)
        lf_trajectory1.points.append(JointTrajectoryPoint())
        lf_trajectory1.points[1].positions = lf_goal1  # [0,0,0,0]
        lf_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RF]
        lf_trajectory1.points[1].accelerations = [0.0 for i in piernas_joints_RF]
        lf_trajectory1.points[1].time_from_start = rospy.Duration(0.5)
        lf_trajectory1.points.append(JointTrajectoryPoint())
        lf_trajectory1.points[2].positions = rf_goal2 # [0,0,0,0]
        lf_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RF]
        lf_trajectory1.points[2].accelerations = [0.0 for i in piernas_joints_RF]
        lf_trajectory1.points[2].time_from_start = rospy.Duration(0.75)
        lf_trajectory1.points.append(JointTrajectoryPoint())
        lf_trajectory1.points[3].positions = rf_goal0  # lf_goal0  # [0,0,0,0]
        lf_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RF]
        lf_trajectory1.points[3].accelerations = [0.0 for i in piernas_joints_RF]
        lf_trajectory1.points[3].time_from_start = rospy.Duration(1.0)
        # '''
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')



        # Create an empty trajectory goal
        rr_goal = FollowJointTrajectoryGoal()
        lm_goal = FollowJointTrajectoryGoal()
        rf_goal = FollowJointTrajectoryGoal()

        lr_goal = FollowJointTrajectoryGoal()
        rm_goal = FollowJointTrajectoryGoal()
        lf_goal = FollowJointTrajectoryGoal()
        # Set the trajectory component to the goal trajectory created above
        rr_goal.trajectory = rr_trajectory1
        lm_goal.trajectory = lm_trajectory1
        rf_goal.trajectory = rf_trajectory1
        lr_goal.trajectory = lr_trajectory1
        rm_goal.trajectory = rm_trajectory1
        lf_goal.trajectory = lf_trajectory1
        # Specify zero tolerance for the execution time
        rr_goal.goal_time_tolerance = rospy.Duration(0.01)
        lm_goal.goal_time_tolerance = rospy.Duration(0.01)
        rf_goal.goal_time_tolerance = rospy.Duration(0.01)
        lr_goal.goal_time_tolerance = rospy.Duration(0.01)
        rm_goal.goal_time_tolerance = rospy.Duration(0.01)
        lf_goal.goal_time_tolerance = rospy.Duration(0.01)
        # Send the goal to the action server
        rr_client.send_goal(rr_goal)
        lm_client.send_goal(lm_goal)
        rf_client.send_goal(rf_goal)
        #rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        lm_client.send_goal(lm_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        lm_client.send_goal(lm_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        lm_client.send_goal(lm_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        lm_client.send_goal(lm_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        lm_client.send_goal(lm_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        lm_client.send_goal(lm_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        lm_client.send_goal(lm_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        lm_client.send_goal(lm_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)

        rr_client.wait_for_result()
        rr_client.send_goal(rr_goal)
        lm_client.send_goal(lm_goal)
        rf_client.send_goal(rf_goal)
        # rr_client.wait_for_result(rospy.Duration(5.0))
        print 'Resultado recibido'
        lr_client.send_goal(lr_goal)
        rm_client.send_goal(rm_goal)
        lf_client.send_goal(lf_goal)
        #lr_client.wait_for_result(rospy.Duration(5.0))
        #rr_client.send_goal(rr_goal)
        #lm_client.send_goal(lm_goal)
        #rf_client.send_goal(rf_goal)'''

        if not sync:
            # Wait for up to 5 seconds for the motion to complete
            rr_client.wait_for_result(rospy.Duration(5.0))

        rr_client.wait_for_result()
        print rr_client.get_result()


        rospy.loginfo('...done')
def forward_kinematics(data):
    if not correct(data):
        rospy.logerr('Incorrect position! ' + str(data))
        return

    kdlChain = kdl.Chain()
    frameFactory = kdl.Frame();

    frame0 = frameFactory.DH(0, 0, 0, 0);
    joint0 = kdl.Joint(kdl.Joint.None)
    kdlChain.addSegment(kdl.Segment(joint0, frame0))

    a, d, al, th = params['i2']
    al, a, d, th = float(al), float(a), float(d), float(th)
    frame1 = frameFactory.DH(a, al, d, th)
    joint1 = kdl.Joint(kdl.Joint.RotZ)
    kdlChain.addSegment(kdl.Segment(joint1, frame1))

    a, d, al, th = params['i1']
    al, a, d, th = float(al), float(a), float(d), float(th)
    frame2 = frameFactory.DH(a, al, d, th)
    joint2 = kdl.Joint(kdl.Joint.RotZ)
    kdlChain.addSegment(kdl.Segment(joint2, frame2))

    a, d, al, th = params['i3']
    al, a, d, th = float(al), float(a), float(d), float(th)
    frame3 = frameFactory.DH(a, al, d, th)
    joint3 = kdl.Joint(kdl.Joint.TransZ)
    kdlChain.addSegment(kdl.Segment(joint3, frame3))

    jntAngles = kdl.JntArray(kdlChain.getNrOfJoints())
    jntAngles[0] = data.position[0]
    jntAngles[1] = data.position[1]
    jntAngles[2] = -data.position[2] - d

    fksolver = kdl.ChainFkSolverPos_recursive(kdlChain)
    eeFrame = kdl.Frame()
    fksolver.JntToCart(jntAngles, eeFrame)
    quaternion = eeFrame.M.GetQuaternion()

    pose = PoseStamped()
    pose.header.frame_id = 'base_link'
    pose.header.stamp = rospy.Time.now()
    pose.pose.position.x = eeFrame.p[0]
    pose.pose.position.y = eeFrame.p[1]
    pose.pose.position.z = eeFrame.p[2]
    pose.pose.orientation.x = quaternion[0]
    pose.pose.orientation.y = quaternion[1]
    pose.pose.orientation.z = quaternion[2]
    pose.pose.orientation.w = quaternion[3]
    pub.publish(pose)

    marker = Marker()
    marker.header.frame_id = 'base_link'
    marker.type = marker.SPHERE
    marker.action = marker.ADD
    marker.pose.orientation.w = 1

    time = rospy.Duration()
    marker.lifetime = time
    marker.scale.x = 0.07
    marker.scale.y = 0.07
    marker.scale.z = 0.07
    marker.pose.position.x = eeFrame.p[0]
    marker.pose.position.y = eeFrame.p[1]
    marker.pose.position.z = eeFrame.p[2]
    marker.pose.orientation.x = quaternion[0]
    marker.pose.orientation.y = quaternion[1]
    marker.pose.orientation.z = quaternion[2]
    marker.pose.orientation.w = quaternion[3]
    marker.color.a = 0.7
    marker.color.r = 0.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    marker_pub.publish(marker)
Esempio n. 12
0
    def compute_balance(self, stance):

        p_c, curr_ori, joint_pos, joint_vel = self.GetState()
        curr_ori = rm.quat_to_rot(curr_ori)

        p_ = []
        for i in range(4):
            frame = kdl.Frame()
            joints = kdl.JntArray(3)
            joints[0] = joint_pos[i + 0]
            joints[1] = joint_pos[i + 1]
            joints[2] = joint_pos[i + 2]
            self.fk_list[i].JntToCart(joints, frame)
            frame_p = np.array([frame.p[0], frame.p[1], frame.p[2]])
            foot_pos = p_c + frame_p
            p_.append(foot_pos)

        acc = np.zeros(3)
        # orientation_error = np.log(ori_des.dot(curr_ori))
        # print("angular accel: {}".format(angular_acc))

        constraints = []
        theta = np.pi / 4
        for s in stance:
            if s:
                cons = np.array([[1, 0,
                                  -np.tan(theta)], [-1, 0, -np.tan(theta)],
                                 [0, 1, -np.tan(theta)],
                                 [0, -1, -np.tan(theta)], [0, 0, -1]])
            else:
                print("error")
                cons = np.array([[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0],
                                 [0, 0, 1], [0, 0, -1]])
            constraints.append(cons)

        eye3 = np.eye(3)
        Atop = np.hstack((eye3, eye3, eye3, eye3))
        Abot = np.hstack((rm.skew_3d(p_[0] - p_c), rm.skew_3d(p_[1] - p_c),
                          rm.skew_3d(p_[2] - p_c), rm.skew_3d(p_[3] - p_c)))
        A = np.vstack([Atop, Abot])

        g = np.array([0, 0, -9.8])
        m = 13.61
        bd_top = m * g
        bd_top = np.reshape(bd_top)
        bd_bot = np.zeros(3)
        bd_bot = np.reshape(bd_bot)
        bd = np.vstack((bd_top, bd_bot))

        P = np.eye(12)

        G = block_diag(*constraints)
        num_h = G.shape[0]
        h = np.zeros(num_h)

        F_opt = rm.quadprog_solve_qp(P, q, G=G, h=h)
        print(F_opt)

        # F_opt = np.linalg.solve(np.dot(A.T, A) + 0.1*np.eye(12),np.dot(A.T, bd))
        # print(F_opt)
        return F_opt
Esempio n. 13
0
 def ik(self, eef_pose):
     q_init = self.joint_state.q
     q_sol = kdl.JntArray(self.num_joints)
     self.pos_ik_solver.CartToJnt(q_init, eef_pose, q_sol)
     return q_sol
Esempio n. 14
0
def callback(data):
    
    kdl_chain = kdl.Chain()
    kdl_frame = kdl.Frame()

    frame0 = kdl_frame.DH(0, 0, 0, 0)
    joint0 = kdl.Joint(kdl.Joint.None)
    kdl_chain.addSegment(kdl.Segment(joint0, frame0))

    a, d, alfa, theta = dh['i2']
    frame1 = kdl_frame.DH(a, alfa, d, theta)
    joint1 = kdl.Joint(kdl.Joint.RotZ)
    kdl_chain.addSegment(kdl.Segment(joint1, frame1))

    a, d, alfa, theta = dh['i1']
    frame2 = kdl_frame.DH(a, alfa, d, theta)
    joint2 = kdl.Joint(kdl.Joint.RotZ)
    kdl_chain.addSegment(kdl.Segment(joint2, frame2))

    a, d, alfa, theta = dh['i3']
    frame3 = kdl_frame.DH(a, alfa, d, theta)
    joint3 = kdl.Joint(kdl.Joint.TransZ)
    kdl_chain.addSegment(kdl.Segment(joint3, frame3))

    joint_angles = kdl.JntArray(kdl_chain.getNrOfJoints())
    joint_angles[0] = data.position[0]
    joint_angles[1] = data.position[1]
    joint_angles[2] = -data.position[2]
    
    kdl_chain_t = kdl.ChainFkSolverPos_recursive(kdl_chain)
    frame_t = kdl.Frame()
    kdl_chain_t.JntToCart(joint_angles, frame_t)
    quat = frame_t.M.GetQuaternion()

    pose = PoseStamped()
    pose.header.frame_id = 'BASE'
    pose.header.stamp = rospy.Time.now()
    pose.pose.position.x = frame_t.p[0]
    pose.pose.position.y = frame_t.p[1]
    pose.pose.position.z = frame_t.p[2]+baseHeight
    pose.pose.orientation.x = quat[0]
    pose.pose.orientation.y = quat[1]
    pose.pose.orientation.z = quat[2]
    pose.pose.orientation.w = quat[3]
    pubP.publish(pose)

    marker = Marker()
    marker.header.frame_id = 'BASE'
    marker.header.stamp = rospy.Time.now()
    marker.type = marker.CUBE
    marker.action = marker.ADD
    marker.pose.orientation.w = 1
    marker.scale.x = 0.2
    marker.scale.y = 0.2
    marker.scale.z = 0.2
    marker.pose.position.x = frame_t.p[0]
    marker.pose.position.y = frame_t.p[1]
    marker.pose.position.z = frame_t.p[2]+baseHeight
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]
    marker.color.a = 0.5
    marker.color.r = 0.5
    marker.color.g = 0.4
    marker.color.b = 1
    pubM.publish(marker)
Esempio n. 15
0
 def _joints_to_kdl(self):
     kdl_array = PyKDL.JntArray(self._num_jnts)
     current_angles = self._limb.joint_angles()
     for i, name in enumerate(self._joint_names):
         kdl_array[i] = current_angles[name]
     return kdl_array
Esempio n. 16
0
def solveIK(targetFrame):
    ok, tree = parser.treeFromFile(rospkg.RosPack().get_path('rviz_animator') + "/models/robocam.xml")
    chain = tree.getChain('base', 'link_camera')

    plotter = Plotter()

    # 1. Solve for J4, J5_initial, J6
    # First, convert quaternion orientation to XZY order Euler angles
    targetQuat = targetFrame.M.GetQuaternion() # Get quaternion from KDL frame (x, y, z, w)
    pitch, yaw, roll = tf.transformations.euler_from_quaternion(targetQuat, axes='rxzy')
    pitch_deg, yaw_deg, roll_deg = math.degrees(pitch), math.degrees(yaw), math.degrees(roll)

    J4_origin_orientation = posemath.toMsg(chain.getSegment(2).getFrameToTip()).orientation
    J4_offset = euler_from_quaternion([J4_origin_orientation.x, J4_origin_orientation.y, J4_origin_orientation.z, J4_origin_orientation.w])[0]

    J4_raw, J5_initial, J6 = pitch, yaw, roll
    J4 = J4_raw - J4_offset
    # 1. Complete above

    print("J4: {} J5_init: {} J6: {}".format(J4, J5_initial, J6))

    chainAngles = PyKDL.JntArray(8)
    chainAngles[5], chainAngles[6], chainAngles[7] = J4, J5_initial, J6
    chainFK = PyKDL.ChainFkSolverPos_recursive(chain)
    purpleFrame = PyKDL.Frame()
    brownFrame = PyKDL.Frame()
    
    purpleSuccess = chainFK.JntToCart(chainAngles, purpleFrame)
    # print("Purple success {}".format(purpleSuccess))

    print("Target Orientation:\n{}".format(targetFrame.M))
    print("Result Orientation:\n{}".format(purpleFrame.M))
    
    brownSuccess = chainFK.JntToCart(chainAngles, brownFrame, segmentNr=7)

    # 2. Determine position of orange point
    # First, construct KDL chain of the 3 links involved in J4-J6
    cameraOffsetChain = tree.getChain('link_pitch', 'link_camera')
    cameraJointAngles = PyKDL.JntArray(2)
    cameraJointAngles[0], cameraJointAngles[1] = J5_initial, J6
    cameraOffsetChainFK = PyKDL.ChainFkSolverPos_recursive(cameraOffsetChain)
    cameraFrame = PyKDL.Frame()
    success = cameraOffsetChainFK.JntToCart(cameraJointAngles, cameraFrame)
    # print("FK Status: {}".format(success))
    # print("Camera Frame: {}".format(cameraFrame))
    # print("End Effector Joint Angles: {}".format([J4, J5_initial, J6]))

    orangePoint = targetFrame.p - (purpleFrame.p - brownFrame.p)

    plotter.addVector(targetFrame.p, "pink")
    plotter.addVector(orangePoint, "orange")
    plotter.addVector(purpleFrame.p, "purple")
    plotter.addVector(brownFrame.p, "brown")

    # print("Target Frame Position: {}".format(targetFrame.p))
    # print("Camera Frame Position: {}".format(cameraFrame.p))
    # print("Offset: {}".format(targetFrame.p - cameraFrame.p))

    # 2. Complete:
    
    # 3. Convert orange point to cylindrical coordinates
    orange_X, orange_Y, orange_Z = orangePoint
    orange_R = math.sqrt(orange_X ** 2 + orange_Y ** 2)
    orange_Theta = math.atan2(orange_Y, orange_X) # Theta measured from global positive X axis
    
    # 3. Complete: (above)

    print("Orange R: {} Theta: {}".format(orange_R, math.degrees(orange_Theta)))

    # 4. Solve for J2 and J3 in the idealized R-Z plane
    targetVectorOrig = PyKDL.Vector(0, orange_R, orange_Z)
    plotter.addVector(targetVectorOrig, "targetRZOrig")

    # First, remove the fixed offsets from the wrist, elbow, and shoulder pieces
    wristEndFrame = PyKDL.Frame()
    wristStartFrame = PyKDL.Frame()
    elbowEndFrame = PyKDL.Frame()
    elbowStartFrame = PyKDL.Frame()
    shoulderEndFrame = PyKDL.Frame()
    shoulderStartFrame = PyKDL.Frame()

    chainFK.JntToCart(chainAngles, wristEndFrame, segmentNr=7)
    chainFK.JntToCart(chainAngles, wristStartFrame, segmentNr=5)
    chainFK.JntToCart(chainAngles, elbowEndFrame, segmentNr=4)
    chainFK.JntToCart(chainAngles, elbowStartFrame, segmentNr=3)
    chainFK.JntToCart(chainAngles, shoulderEndFrame, segmentNr=2)
    chainFK.JntToCart(chainAngles, shoulderStartFrame, segmentNr=0)

    plotter.addVector(wristEndFrame.p, "wristEndFrame")
    plotter.addVector(wristStartFrame.p, "wristStartFrame")
    plotter.addVector(elbowEndFrame.p, "elbowEndFrame")
    plotter.addVector(elbowStartFrame.p, "elbowStartFrame")
    plotter.addVector(shoulderEndFrame.p, "shoulderEndFrame")
    plotter.addVector(shoulderStartFrame.p, "shoulderStartFrame")

    wristOffset = wristEndFrame.p - wristStartFrame.p
    elbowOffset = elbowEndFrame.p - elbowStartFrame.p
    shoulderOffset = shoulderEndFrame.p - shoulderStartFrame.p
    targetVector = targetVectorOrig - wristOffset - elbowOffset - shoulderOffset

    plotter.addVector(targetVector, "targetRZ")

    # The following steps use the same labels as the classic 2D planar IK derivation
    a1, a2 = (shoulderEndFrame.p - elbowStartFrame.p).Norm(), (elbowEndFrame.p - wristStartFrame.p).Norm()
    _, ik_x, ik_y = targetVector
    
    q2_a = math.acos((ik_x ** 2 + ik_y ** 2 - a1 ** 2 - a2 ** 2) / (2 * a1 * a2))
    q1_a = math.atan2(ik_y, ik_x) - math.atan2(a2 * math.sin(-q2_a), a1 + a2 * math.cos(-q2_a))

    q2_b = -1 * math.acos((ik_x ** 2 + ik_y ** 2 - a1 ** 2 - a2 ** 2) / (2 * a1 * a2))
    q1_b = math.atan2(ik_y, ik_x) + math.atan2(a2 * math.sin(q2_b), a1 + a2 * math.cos(q2_b))

    # Choose 'better' set of q1_ab, q2_ab
    q1, q2 = q1_a, q2_a # TODO(JS): Is this always the better one?

    J2_initial = q1
    J2_offset = math.radians(90) # J2's zero position is vertical, not horizontal
    J2 = J2_initial - J2_offset

    # Since we have a parallel link, the angle for J3 is not simply q2. Instead, use transversal
    J3_initial = q1 - q2
    J3_offset = elbowStartFrame.M.GetRPY()[0] # J3's zero position is below horizontal
    J3 = J3_initial - J3_offset
    # 4. Complete (above)

    # print("J2: {} J3: {}".format(J2, J3))
    
    # 5. Use the Theta from cylindrical coordinates as the J1 angle, and update J5 accordingly
    J1 = orange_Theta - math.radians(90)
    J5 = J5_initial - (orange_Theta - math.radians(90))
    # 5. Complete (above)
    
    # print("J1: {} J5: {}".format(J1, J5))

    jointAngles = [J1, J2, J3, J4, J5, J6]
    jointNames = ['joint_base_rot', 'joint_rot_1', 'joint_f1_2', 'joint_f2_pitch', 'joint_pitch_yaw', 'joint_yaw_roll']

    print("\n\nFinal joint angles in radians: {}\n\n".format(jointAngles))

    in_bounds = True
    for angle, name in zip(jointAngles, jointNames):
        limit = robot_urdf.joint_map[name].limit
        if angle < limit.lower or angle > limit.upper:
            in_bounds = False
            print("Joint {} out of bounds with angle {} not between {} and {}".format(name, angle, limit.lower, limit.upper))
    print("All in bounds: {}".format(in_bounds))        

    solvedJoints = PyKDL.JntArray(8)
    solvedJoints[0], solvedJoints[1], solvedJoints[3], solvedJoints[5], solvedJoints[6], solvedJoints[7] = jointAngles
    solvedJoints[2], solvedJoints[4] = -1 * solvedJoints[1], -1 * solvedJoints[3]
    producedFrame = PyKDL.Frame()

    for i in range(chain.getNrOfSegments()):
        rc = chainFK.JntToCart(solvedJoints, producedFrame, segmentNr=i)
        plotter.addVector(producedFrame.p, "fk_produced_{}".format(i))

    # print("Result: {}".format(rc))
    # print("Output position: {}\nExpected position: {}".format(producedFrame.p, targetFrame.p))
    # print("Output orientation: {}\nExpected orientation: {}".format(producedFrame.M, targetFrame.M))


    # 6. (optional) Sanity check on solution:
    # sanityTest(BASE_TO_BASE_YAW, BASE_YAW_TO_BOTTOM_4B, targetFrame, cameraFrame, cameraOffsetChain, jointAngles)

    # 7. Create JointState message for return
    ret = JointState()
    ret.name = ['joint_base_rot', 'joint_rot_1', 'joint_f1_2', 'joint_f2_pitch', 'joint_pitch_yaw', 'joint_yaw_roll']
    ret.header.stamp = rospy.Time.now()
    ret.position = jointAngles
    
    plotter.addGoal(ret)
    # plotter.publish()

    return in_bounds, ret
Esempio n. 17
0
import rospy
import kdl_parser_py.urdf
import PyKDL as kdl
import math
import actionlib
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal

if __name__ == "__main__":
    rospy.init_node("py_kdl_ik")
    robot_description = rospy.get_param("robot_description")
    (ok, tree) = kdl_parser_py.urdf.treeFromString(robot_description)
    joint_names = rospy.get_param("/crane_x7/arm_controller/joints")
    chain = tree.getChain("base_link", "crane_x7_gripper_base_link")
    solver = kdl.ChainIkSolverPos_LMA(chain)
    q_init = kdl.JntArray(chain.getNrOfJoints())
    p_out = kdl.Frame(kdl.Rotation.RPY(0.0, math.radians(90.0), 0.0),
                      kdl.Vector(0.0, 0.0, 0.32))
    q_sol = kdl.JntArray(chain.getNrOfJoints())

    ret = solver.CartToJnt(q_init, p_out, q_sol)
    if ret != 0:
        rospy.logerr("IK Failed")
        rospy.signal_shutdown("IK Failed")

    ac = actionlib.SimpleActionClient(
        "/crane_x7/arm_controller/follow_joint_trajectory",
        FollowJointTrajectoryAction)
    if not ac.wait_for_server(timeout=rospy.Duration(3.0)):
        rospy.logerr("Cannot found action server")
        rospy.signal_shutdown("Cannot found action server")
Esempio n. 18
0
    flg.append(True)


def callback(data):
    caliculate(data.data[0], data.data[1], data.data[2])


rospy.init_node('inverse_kinematics', anonymous=True)

rospy.Subscriber("chatter", Float32MultiArray, callback)
pub = rospy.Publisher('joint_states', JointState, queue_size=10)

position = [2] * 5
position_old = [2] * 5
q_zero = None
q_zero = kdl.JntArray(chain.getNrOfJoints())
q_solved = None


def caliculate(data0, data1, data2):
    global q_zero
    global q_solved
    print q_zero
    F2 = kdl.Frame(kdl.Rotation.Quaternion(0, -0.29552, 0, 0.955337),
                   kdl.Vector(data0, data1, data2))
    global position
    global position_old
    # print position[0]
    #print position_old[0]
    for i in range(5):
        if flg[i] == True:
def callback(data):
    kdl_chain =PyKDL.Chain()   
    Frame = PyKDL.Frame();

    i = 0
    d=0
    th=0
    for joint in dh_file:
        name = joint['name']
        last_d = d
        last_th = th
        a = joint["a"]
        d = joint["d"]
        al = joint["al"]
        th = joint["th"]

        if name == 'i3' and get_parameters('dlugosc1'):
        	a = rospy.get_param("/dlugosc1")

        if name == 'hand' and get_parameters('dlugosc2'):
            a = rospy.get_param("/dlugosc2")

        #forego first iteration
        if i==0:
            i = i+1
            continue
        
        kdl_chain.addSegment(PyKDL.Segment(PyKDL.Joint(PyKDL.Joint.RotZ), Frame.DH(a, al, last_d, last_th)))
        
        i = i+1
    
    jointDisplacement = PyKDL.JntArray(kdl_chain.getNrOfJoints())
    
    #joint displacements including restrictions
    jointDisplacement[0] = data.position[0]
    jointDisplacement[1] = data.position[1]
    jointDisplacement[2] = data.position[2]

    if(data.position[0] < restrictions[0]['backward']):
        jointDisplacement[0] = restrictions[0]['backward']
        rospy.logerr("[KDL] Przekroczono ograniczenie dolne stawu o numerze: 1")
    elif(data.position[0] > restrictions[0]['forward']):
        jointDisplacement[0] = restrictions[0]['forward']
        rospy.logerr("[KDL] Przekroczono ograniczenie gorne stawu o numerze: 1")

    if(data.position[1] < restrictions[1]['backward']):
        jointDisplacement[1] = restrictions[1]['backward']
        rospy.logerr("[KDL] Przekroczono ograniczenie dolne stawu o numerze: 2")
    elif(data.position[1] > restrictions[1]['forward']):
        jointDisplacement[1] = restrictions[1]['forward']
        rospy.logerr("[KDL] Przekroczono ograniczenie gorne stawu o numerze: 2")

    if(data.position[2] < restrictions[2]['backward']):
        jointDisplacement[2] = restrictions[2]['backward']
        rospy.logerr("[KDL] Przekroczono ograniczenie dolne stawu o numerze: 3")
    elif(data.position[2] > restrictions[2]['forward']):
        jointDisplacement[2] = restrictions[2]['forward']
        rospy.logerr("[KDL] Przekroczono ograniczenie gorne stawu o numerze: 3")

    f_k_solver = PyKDL.ChainFkSolverPos_recursive(kdl_chain)

    frame = PyKDL.Frame()
    f_k_solver.JntToCart(jointDisplacement, frame)
    quatr = frame.M.GetQuaternion()
    
    kdl_pose = PoseStamped()
    kdl_pose.header.frame_id = 'base_link'
    kdl_pose.header.stamp = rospy.Time.now()

    kdl_pose.pose.position.x = frame.p[0]
    kdl_pose.pose.position.y = frame.p[1]
    kdl_pose.pose.position.z = frame.p[2]
    kdl_pose.pose.orientation.x = quatr[0]
    kdl_pose.pose.orientation.y = quatr[1]
    kdl_pose.pose.orientation.z = quatr[2]
    kdl_pose.pose.orientation.w = quatr[3]
    
    pub.publish(kdl_pose)
Esempio n. 20
0
    def __init__(self):
        #filename = '/home/kaiser/WS_ROS/catkin_ws/src/abb_experimental-indigo-devel/abb_irb120_gazebo/urdf/modelo_exportado.urdf'
        filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/IRB120_URDF_v2/robots/IRB120_URDF_v2.URDF'
        robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)

        (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
        chain = tree.getChain("base_link", "link_6")
        print chain.getNrOfSegments()
        solverCinematicaDirecta = PyKDL.ChainFkSolverPos_recursive(chain)
        nj_izq = chain.getNrOfJoints()
        posicionInicial = PyKDL.JntArray(nj_izq)
        posicionInicial[0] = 0
        posicionInicial[1] = 0
        posicionInicial[2] = 0
        posicionInicial[3] = 0
        posicionInicial[4] = 0
        posicionInicial[5] = 0
        print "Forward kinematics"
        finalFrame = PyKDL.Frame()
        solverCinematicaDirecta.JntToCart(posicionInicial, finalFrame)
        print "Rotational Matrix of the final Frame: "
        print  finalFrame.M
        print "End-effector position: ", finalFrame.p

        rospy.init_node('trajectory_demo')

        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)

        # Set to False to wait for arm to finish before moving head
        sync = rospy.get_param('~sync', True)

        # Which joints define the arm?
        arm_joints = ['joint_1',
                      'joint_2',
                      'joint_3',
                      'joint_4',
                      'joint_5',
                      'joint_6']


        if reset:
            # Set the arm back to the resting position
            arm_goal  = [0, 0, 0, 0, 0, 0]

        else:
            # Set a goal configuration for the arm
            arm_goal  = [-0.3, 0.5, -1.0, 1.8, 0.3, 0.6]

        print "Inverse Kinematics"
        q_init = posicionInicial  # initial angles
        vik = PyKDL.ChainIkSolverVel_pinv(chain)
        ik = PyKDL.ChainIkSolverPos_NR(chain, solverCinematicaDirecta, vik)
        #desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame = finalFrame
        desiredFrame.p[0] = finalFrame.p[0]
        desiredFrame.p[1] = finalFrame.p[1]
        desiredFrame.p[2] = finalFrame.p[2]-0.25
        print "Desired Position: ", desiredFrame.p
        q_out = PyKDL.JntArray(6)
        ik.CartToJnt(q_init, desiredFrame, q_out)
        print "Output angles in rads: ", q_out

        arm_goal[0] = q_out[0]
        arm_goal[1] = q_out[1]
        arm_goal[2] = q_out[2]
        arm_goal[3] = q_out[3]
        arm_goal[4] = q_out[4]
        arm_goal[5] = q_out[5]


        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')

        pub = rospy.Publisher('joint_path_command',JointTrajectory,queue_size=10)

        #arm_client = actionlib.SimpleActionClient('joint_trajectory_action', FollowJointTrajectoryAction)

        #arm_client.wait_for_server()

        rospy.loginfo('...connected.')


        # Create a single-point arm trajectory with the arm_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(2.0)

        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')

        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()

        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory

        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0.0)

        # Send the goal to the action server
        #arm_client.send_goal(arm_goal)

        pub.publish(arm_trajectory)

        while not rospy.is_shutdown():
            pub.publish(arm_trajectory)
            rospy.sleep(rospy.Duration(0.5))

        #if not sync:
            # Wait for up to 5 seconds for the motion to complete
            #arm_client.wait_for_result(rospy.Duration(5.0))


        rospy.loginfo('...done')
Esempio n. 21
0
snake = u2c.URDFparser()
snake.from_file(path_to_urdf)

#pybullet
sim = pb.connect(pb.DIRECT)
snake_pb = pb.loadURDF(path_to_urdf, useFixedBase=True, flags = pb.URDF_USE_INERTIA_FROM_FILE)

#joint info
jointlist, names, q_max, q_min = snake.get_joint_info(root, tip)
n_joints = snake.get_n_joints(root, tip)


#declarations

#kdl
q_kdl = kdl.JntArray(n_joints)
qdot_kdl = kdl.JntArray(n_joints)
gravity_kdl = kdl.Vector()
gravity_kdl[2] = -9.81
C_kdl = kdl.JntArray(n_joints)
g_kdl = kdl.JntArray(n_joints)

#u2c & pybullet
q = [None]*n_joints
qdot = [None]*n_joints
zeros_pb = [None]*n_joints
gravity_u2c = [0, 0, -9.81]
g_sym = snake.get_gravity_rnea(root, tip, gravity_u2c)
C_sym = snake.get_coriolis_rnea(root, tip)

Esempio n. 22
0
from urdf_parser_py.urdf import URDF
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model

robot = URDF.from_xml_file("/home/d/catkin_ws/src/robot_description/armc_description/urdf/armc_description.urdf")
tree = kdl_tree_from_urdf_model(robot)

print tree.getNrOfSegments()
chain = tree.getChain("base_link", "sensor_link")
print chain.getNrOfSegments()
print chain.getNrOfJoints()

#正运动学
fk = PyKDL.ChainFkSolverPos_recursive(chain)

pos = PyKDL.Frame()
q = PyKDL.JntArray(7)
for i in range(7):
    q[i] = 0
q[0] = 1
fk_flag = fk.JntToCart(q, pos)
print "fk_flag", fk_flag
print "pos", pos

#逆运动学
ik_v = PyKDL.ChainIkSolverVel_pinv(chain)
ik = PyKDL.ChainIkSolverPos_NR(chain, fk, ik_v, maxiter=100, eps=math.pow(10, -9))

qq = PyKDL.JntArray(7)
qq_k = PyKDL.JntArray(7)
ik.CartToJnt(qq_k, pos, qq)
print "qq:", qq
Esempio n. 23
0
    def inverse_kinematics(self,
                           position,
                           orientation=None,
                           seed=None,
                           min_joints=None,
                           max_joints=None,
                           w_x=None,
                           w_q=None,
                           maxiter=500,
                           eps=1.0e-6,
                           with_st=False):
        if w_x is None and w_q is None:
            ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
        else:
            ik_v_kdl = PyKDL.ChainIkSolverVel_wdls(self._arm_chain)
            if w_x is not None: ik_v_kdl.setWeightTS(w_x)  #TS = Task Space
            if w_q is not None: ik_v_kdl.setWeightJS(w_q)  #JS = Joint Space
        pos = PyKDL.Vector(position[0], position[1], position[2])
        if orientation is not None:
            rot = PyKDL.Rotation()
            rot = rot.Quaternion(orientation[0], orientation[1],
                                 orientation[2], orientation[3])
        # Populate seed with current angles if not provided
        seed_array = PyKDL.JntArray(self._num_jnts)
        if seed is not None:
            seed_array.resize(len(seed))
            for idx, jnt in enumerate(seed):
                seed_array[idx] = jnt
        else:
            seed_array = self.joints_to_kdl('positions')

        # Make IK Call
        if orientation is not None:
            goal_pose = PyKDL.Frame(rot, pos)
        else:
            goal_pose = PyKDL.Frame(pos)
        result_angles = PyKDL.JntArray(self._num_jnts)

        # Make IK solver with joint limits
        if min_joints is None: min_joints = self.joint_limits_lower
        if max_joints is None: max_joints = self.joint_limits_upper
        mins_kdl = PyKDL.JntArray(len(min_joints))
        for idx, jnt in enumerate(min_joints):
            mins_kdl[idx] = jnt
        maxs_kdl = PyKDL.JntArray(len(max_joints))
        for idx, jnt in enumerate(max_joints):
            maxs_kdl[idx] = jnt
        ik_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl,
                                                maxs_kdl, self._fk_p_kdl,
                                                ik_v_kdl, maxiter, eps)

        if ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0:
            result = np.array(list(result_angles))
            if with_st: return True, result
            else: return result
        else:
            if with_st:
                result = np.array(list(result_angles))
                return False, result
            else:
                return None
Esempio n. 24
0
 def NpToJnt(self, q):
     temp = PyKDL.JntArray(self.nj)
     for j in range(self.nj):
         temp[j] = q[j]
     return temp
Esempio n. 25
0
    def __init__(self):

        pi4 = 0.7853

        filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/mi_robot_xacro4_cambio.urdf'

        robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)

        (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
        cadena_der_up_down = tree.getChain("base_link", "pie_der_link")
        cadena_der_down_up = tree.getChain("pie_der_link", "base_link")
        cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link")
        cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link")

        print cadena_der_up_down.getNrOfSegments()
        fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_der_up_down)
        fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_der_down_up)
        fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_izq_up_down)
        fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_izq_down_up)

        vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down)
        ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down, fksolver_der_up_down, vik_der_up_down)

        vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up)
        ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up, fksolver_der_down_up, vik_der_down_up)

        vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down)
        ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down, fksolver_izq_up_down, vik_izq_up_down)

        vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up)
        ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up, fksolver_izq_down_up, vik_izq_down_up)

        nj_izq = cadena_der_up_down.getNrOfJoints()
        posicionInicial_der_up_down = PyKDL.JntArray(nj_izq)
        posicionInicial_der_up_down[0] = 0
        posicionInicial_der_up_down[1] = 0.3
        posicionInicial_der_up_down[2] = -0.3
        posicionInicial_der_up_down[3] = 0.6
        posicionInicial_der_up_down[4] = -0.3
        posicionInicial_der_up_down[5] = -0.3

        nj_izq = cadena_izq_up_down.getNrOfJoints()
        posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq)
        posicionInicial_izq_up_down[0] = 0
        posicionInicial_izq_up_down[1] = 0.3
        posicionInicial_izq_up_down[2] = -0.3
        posicionInicial_izq_up_down[3] = 0.6
        posicionInicial_izq_up_down[4] = -0.3
        posicionInicial_izq_up_down[5] = -0.3

        nj_izq = cadena_der_down_up.getNrOfJoints()
        posicionInicial_der_down_up = PyKDL.JntArray(nj_izq)
        posicionInicial_der_down_up[5] = 0
        posicionInicial_der_down_up[4] = 0.3
        posicionInicial_der_down_up[3] = -0.3
        posicionInicial_der_down_up[2] = 0.6
        posicionInicial_der_down_up[1] = -0.3
        posicionInicial_der_down_up[0] = -0.3

        nj_izq = cadena_izq_down_up.getNrOfJoints()
        posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq)
        posicionInicial_izq_down_up[5] = 0
        posicionInicial_izq_down_up[4] = 0.3
        posicionInicial_izq_down_up[3] = -0.3
        posicionInicial_izq_down_up[2] = 0.6
        posicionInicial_izq_down_up[1] = -0.3
        posicionInicial_izq_down_up[0] = -0.3
        print "Forward kinematics"
        finalFrame_izq_up_down = PyKDL.Frame()
        finalFrame_izq_down_up = PyKDL.Frame()
        finalFrame_der_up_down = PyKDL.Frame()
        finalFrame_der_down_up = PyKDL.Frame()


        print "Rotational Matrix of the final Frame: "
        print  finalFrame_izq_up_down.M
        print "End-effector position: ", finalFrame_izq_up_down.p

        rospy.init_node('trajectory_demo')

        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)

        # Set to False to wait for arm to finish before moving head
        sync = rospy.get_param('~sync', True)

        # Which joints define the arm?
        piernas_joints = ['cilinder_blue_box1_der_joint', 'cilinder_blue1_box2_der_joint','cilinder_blue_box2_der_joint',
                          'cilinder_blue_box4_der_joint', 'cilinder_blue1_box6_der_joint','cilinder_blue_box6_der_joint',
                          'cilinder_blue_box1_izq_joint', 'cilinder_blue1_box2_izq_joint','cilinder_blue_box2_izq_joint',
                          'cilinder_blue_box4_izq_joint', 'cilinder_blue1_box6_izq_joint','cilinder_blue_box6_izq_joint']

        piernas_goal0  = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        #11111111111111111111111111111111111111111111
        print "Inverse Kinematics"
        fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down, finalFrame_izq_up_down)
        fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up)
        q_init_izq_up_down = posicionInicial_izq_up_down  # initial angles
        desiredFrame = finalFrame_izq_up_down
        desiredFrame.p[0] = finalFrame_izq_up_down.p[0]
        desiredFrame.p[1] = finalFrame_izq_up_down.p[1]
        desiredFrame.p[2] = finalFrame_izq_up_down.p[2]
        print "Desired Position: ", desiredFrame.p
        q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
        ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down)
        print "Output angles in rads: ", q_out_izq_up_down



        print "Inverse Kinematics"
        fksolver_der_up_down.JntToCart(posicionInicial_der_up_down, finalFrame_der_up_down)
        fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up)
        q_init_der_up_down = posicionInicial_der_up_down  # initial angles
        # desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
        desiredFrame = finalFrame_der_up_down
        desiredFrame.p[0] = finalFrame_der_up_down.p[0]
        desiredFrame.p[1] = finalFrame_der_up_down.p[1]
        desiredFrame.p[2] = finalFrame_der_up_down.p[2]+0.02 #0.02
        print "Desired Position: ", desiredFrame.p
        q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
        ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame, q_out_der_up_down)
        print "Output angles in rads: ", q_out_der_up_down

        piernas_goal0[6] = q_out_izq_up_down[0]
        piernas_goal0[7] = q_out_izq_up_down[1]
        piernas_goal0[8] = q_out_izq_up_down[2]
        piernas_goal0[9] = q_out_izq_up_down[3]
        piernas_goal0[10] = q_out_izq_up_down[4]
        piernas_goal0[11] = q_out_izq_up_down[5]

        piernas_goal0[0] = q_out_der_up_down[0]
        piernas_goal0[1] = q_out_der_up_down[1]
        piernas_goal0[2] = q_out_der_up_down[2]
        piernas_goal0[3] = q_out_der_up_down[3]
        piernas_goal0[4] = q_out_der_up_down[4]
        piernas_goal0[5] = q_out_der_up_down[5]

        #121212122121212121212121212121212121212121212
        piernas_goal12 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        #q_out_izq_up_down[0] -= pi4
        piernas_goal12[6] = q_out_izq_up_down[0]-2*pi4
        piernas_goal12[7] = q_out_izq_up_down[1]
        piernas_goal12[8] = q_out_izq_up_down[2]
        piernas_goal12[9] = q_out_izq_up_down[3]
        piernas_goal12[10] = q_out_izq_up_down[4]
        piernas_goal12[11] = q_out_izq_up_down[5]

        piernas_goal12[0] = 0
        piernas_goal12[1] = 0
        piernas_goal12[2] = -0.7
        piernas_goal12[3] = 1.4
        piernas_goal12[4] = 0
        piernas_goal12[5] = -0.7

        #########################################################

        piernas_goal2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        piernas_goal2[6] = q_out_izq_up_down[0]-2*pi4
        piernas_goal2[7] = q_out_izq_up_down[1]-0.3-0.3
        piernas_goal2[8] = q_out_izq_up_down[2]-0.3
        piernas_goal2[9] = q_out_izq_up_down[3]+0.6
        piernas_goal2[10] = q_out_izq_up_down[4]+0.3+0.3
        piernas_goal2[11] = q_out_izq_up_down[5] -0.3

        piernas_goal2[0] = 0#-pi4
        piernas_goal2[1] = -0.25
        piernas_goal2[2] = -0.3
        piernas_goal2[3] = 0.6
        piernas_goal2[4] = 0.25
        piernas_goal2[5] = -0.3

        ##################################################
        piernas_goal3 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        piernas_goal3[6] = q_out_izq_up_down[0]   +0.0
        piernas_goal3[7] = q_out_izq_up_down[1] - 0.3
        piernas_goal3[8] = q_out_izq_up_down[2] - 0.3
        piernas_goal3[9] = q_out_izq_up_down[3] + 0.6
        piernas_goal3[10] = q_out_izq_up_down[4] + 0.3
        piernas_goal3[11] = q_out_izq_up_down[5] - 0.3

        piernas_goal3[0] = -2*pi4
        piernas_goal3[1] = -0.25
        piernas_goal3[2] = -0.3
        piernas_goal3[3] = 0.6
        piernas_goal3[4] = 0.25
        piernas_goal3[5] = -0.3

        ##################################################
        piernas_goal4 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        piernas_goal4[6] = q_out_izq_up_down[0] +0.0
        piernas_goal4[7] = q_out_izq_up_down[1] - 0.3  +0.3
        piernas_goal4[8] = q_out_izq_up_down[2] - 0.3   +0.3
        piernas_goal4[9] = q_out_izq_up_down[3] + 0.6  -0.6
        piernas_goal4[10] = q_out_izq_up_down[4] + 0.3 -0.3
        piernas_goal4[11] = q_out_izq_up_down[5] - 0.3 +0.3

        piernas_goal4[0] = -2*pi4
        piernas_goal4[1] = -0.25
        piernas_goal4[2] = -0.7
        piernas_goal4[3] = 1.4
        piernas_goal4[4] = 0.25
        piernas_goal4[5] = -0.7



        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for right arm trajectory controller...')

        arm_client = actionlib.SimpleActionClient('/piernas/piernas_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
        #/piernas/piernas_controller/follow_joint_trajectory
        arm_client.wait_for_server()

        rospy.loginfo('...connected.')



        # Create a single-point arm trajectory with the piernas_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = piernas_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = piernas_goal0
        arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[1].positions = piernas_goal12
        arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[1].time_from_start = rospy.Duration(6.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[2].positions = piernas_goal2
        arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[2].time_from_start = rospy.Duration(9.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[3].positions = piernas_goal3
        arm_trajectory.points[3].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[3].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[3].time_from_start = rospy.Duration(12.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[4].positions = piernas_goal4
        arm_trajectory.points[4].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[4].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[4].time_from_start = rospy.Duration(15.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[5].positions = piernas_goal12
        arm_trajectory.points[5].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[5].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[5].time_from_start = rospy.Duration(18.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[6].positions = piernas_goal2
        arm_trajectory.points[6].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[6].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[6].time_from_start = rospy.Duration(21.0)

        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[7].positions = piernas_goal3
        arm_trajectory.points[7].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[7].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[7].time_from_start = rospy.Duration(24)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[8].positions = piernas_goal4
        arm_trajectory.points[8].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[8].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[8].time_from_start = rospy.Duration(27)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[9].positions = piernas_goal12
        arm_trajectory.points[9].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[9].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[9].time_from_start = rospy.Duration(30.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[10].positions = piernas_goal2
        arm_trajectory.points[10].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[10].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[10].time_from_start = rospy.Duration(33.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[11].positions = piernas_goal3
        arm_trajectory.points[11].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[11].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[11].time_from_start = rospy.Duration(36.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[12].positions = piernas_goal4
        arm_trajectory.points[12].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[12].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[12].time_from_start = rospy.Duration(39.0)
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[13].positions = piernas_goal12
        arm_trajectory.points[13].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[13].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[13].time_from_start = rospy.Duration(42.0)
        '''arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[14].positions = piernas_goal4
        arm_trajectory.points[14].velocities = [0.0 for i in piernas_joints]
        arm_trajectory.points[14].accelerations = [0.0 for i in piernas_joints]
        arm_trajectory.points[14].time_from_start = rospy.Duration(32.0)'''
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')



        # Create an empty trajectory goal
        piernas_goal = FollowJointTrajectoryGoal()

        # Set the trajectory component to the goal trajectory created above
        piernas_goal.trajectory = arm_trajectory

        # Specify zero tolerance for the execution time
        piernas_goal.goal_time_tolerance = rospy.Duration(0.01)

        # Send the goal to the action server
        arm_client.send_goal(piernas_goal)

        if not sync:
            # Wait for up to 5 seconds for the motion to complete
            arm_client.wait_for_result(rospy.Duration(5.0))

        arm_client.wait_for_result()
        print arm_client.get_result()


        rospy.loginfo('...done')
#pybullet
sim = pb.connect(pb.DIRECT)
snake_pb = pb.loadURDF(path_to_urdf,
                       useFixedBase=True,
                       flags=pb.URDF_USE_INERTIA_FROM_FILE)
pb.setGravity(0, 0, -9.81)

#joint info
jointlist, names, q_max, q_min = snake.get_joint_info(root, tip)
n_joints = snake.get_n_joints(root, tip)

#declarations

#kdl
q_kdl = kdl.JntArray(n_joints)
gravity_kdl = kdl.Vector()
gravity_kdl[2] = -9.81
g_kdl = kdl.JntArray(n_joints)
M_kdl = kdl.JntSpaceInertiaMatrix(n_joints)

#u2c & pybullet
q = [None] * n_joints
qdot = [None] * n_joints
zeros_pb = [None] * n_joints
M_sym = snake.get_inertia_matrix_crba(root, tip)

#rbdl
q_np = np.zeros(n_joints)
M_rbdl = (n_joints, n_joints)
M_rbdl = np.zeros(M_rbdl)
Esempio n. 27
0
    def gravity(self, q):
        g_kdl = kdl.JntArray(self.num_joints)

        self._dyn_kdl.JntToGravity(joint_list_to_kdl(q), g_kdl)
        return joint_kdl_to_list(g_kdl)