Python chain_from_param 예제들

예제 #1

파일: pr2_arm.py 프로젝트: rgleichman/berkeley_demos

def create_pr2_arm(arm, arm_type=PR2Arm, base_link="torso_lift_link",  
                   end_link="%s_gripper_tool_frame", param="/robot_description",
                   controller_name=None, timeout=5.):
    if "%s" in base_link:
        base_link = base_link % arm
    if "%s" in end_link:
        end_link = end_link % arm
    chain, joint_info = kdlp.chain_from_param(base_link, end_link, param=param)
    kin = KDLArmKinematics(chain, joint_info, base_link, end_link)
    if controller_name is None:
        return arm_type(arm, kin)
    else:
        return arm_type(arm, kin, controller_name=controller_name)

예제 #2

def create_pr2_arm(arm,
                   arm_type=PR2Arm,
                   base_link="torso_lift_link",
                   end_link="%s_gripper_tool_frame",
                   param="/robot_description",
                   controller_name=None,
                   timeout=5.):
    if "%s" in base_link:
        base_link = base_link % arm
    if "%s" in end_link:
        end_link = end_link % arm
    chain, joint_info = kdlp.chain_from_param(base_link, end_link, param=param)
    kin = KDLArmKinematics(chain, joint_info, base_link, end_link)
    if controller_name is None:
        return arm_type(arm, kin)
    else:
        return arm_type(arm, kin, controller_name=controller_name)

예제 #3

파일: test_kdl_parser.py 프로젝트: rgleichman/berkeley_demos

#! /usr/bin/python

import numpy as np

import roslib
roslib.load_manifest("kdl_parser_python")
roslib.load_manifest("hrl_kdl_arms")
import rospy
import urdf_parser_python.urdf_parser as urdf
import kdl_parser_python.kdl_parser as kdlp
import PyKDL as kdl
from hrl_kdl_arms.kdl_arm_kinematics import KDLArmKinematics

chain, joint_info = kdlp.chain_from_param("torso_lift_link", "r_gripper_tool_frame")

print "Joint Information:", joint_info

kinematics = KDLArmKinematics(chain=chain)
print kinematics.chain.getNrOfSegments()
while not rospy.is_shutdown():
    q = np.random.uniform(-0.3, 0.3, 7)
    print "FK", kinematics.FK_vanilla(q, 11)
    J = kinematics.jacobian_vanilla(q)
    print "Jacobian", J
    rospy.sleep(0.2)

예제 #4

파일: test_kdl_parser.py 프로젝트: wklharry/hrl

import numpy as np

import roslib
roslib.load_manifest("hrl_kdl_arms")
roslib.load_manifest("equilibrium_point_control")
roslib.load_manifest("rospy")
roslib.load_manifest("urdf_parser_python")
roslib.load_manifest("kdl_parser_python")
#roslib.load_manifest("python_orocos_kdl")
import rospy
import urdf_parser_python.urdf_parser as urdf
import kdl_parser_python.kdl_parser as kdlp
import PyKDL as kdl
from hrl_kdl_arms.kdl_arm_kinematics import KDLArmKinematics

chain = kdlp.chain_from_param("torso_lift_link", "r_gripper_tool_frame")

if True:
    kinematics = KDLArmKinematics(chain=chain)
    while not rospy.is_shutdown():
        q = np.random.uniform(-0.3, 0.3, 7)
        print kinematics.chain.getNrOfSegments()
        print kinematics.FK_vanilla(q, 11)
        H = kinematics.inertia(q)
        J = kinematics.jacobian_vanilla(q)
        print H.shape, H
        print np.linalg.inv(J * np.linalg.inv(H) * J.T)
        print np.log10(np.linalg.cond(H))
        print np.log10(np.linalg.cond(J * np.linalg.inv(H) * J.T))
        print np.log10(np.linalg.cond(kinematics.cart_inertia(q)))
        rospy.sleep(0.2)

예제 #5

import roslib

roslib.load_manifest("hrl_kdl_arms")
roslib.load_manifest("equilibrium_point_control")
roslib.load_manifest("rospy")
roslib.load_manifest("urdf_parser_python")
roslib.load_manifest("kdl_parser_python")
# roslib.load_manifest("python_orocos_kdl")
import rospy
import urdf_parser_python.urdf_parser as urdf
import kdl_parser_python.kdl_parser as kdlp
import PyKDL as kdl
from hrl_kdl_arms.kdl_arm_kinematics import KDLArmKinematics

chain = kdlp.chain_from_param("torso_lift_link", "r_gripper_tool_frame")

if True:
    kinematics = KDLArmKinematics(chain=chain)
    while not rospy.is_shutdown():
        q = np.random.uniform(-0.3, 0.3, 7)
        print kinematics.chain.getNrOfSegments()
        print kinematics.FK_vanilla(q, 11)
        H = kinematics.inertia(q)
        J = kinematics.jacobian_vanilla(q)
        print H.shape, H
        print np.linalg.inv(J * np.linalg.inv(H) * J.T)
        print np.log10(np.linalg.cond(H))
        print np.log10(np.linalg.cond(J * np.linalg.inv(H) * J.T))
        print np.log10(np.linalg.cond(kinematics.cart_inertia(q)))
        rospy.sleep(0.2)

예제 #6

파일: test_kdl_parser.py 프로젝트: rgleichman/berkeley_demos

#! /usr/bin/python

import numpy as np

import roslib
roslib.load_manifest("kdl_parser_python")
roslib.load_manifest("hrl_kdl_arms")
import rospy
import urdf_parser_python.urdf_parser as urdf
import kdl_parser_python.kdl_parser as kdlp
import PyKDL as kdl
from hrl_kdl_arms.kdl_arm_kinematics import KDLArmKinematics

chain, joint_info = kdlp.chain_from_param("torso_lift_link",
                                          "r_gripper_tool_frame")

print "Joint Information:", joint_info

kinematics = KDLArmKinematics(chain=chain)
print kinematics.chain.getNrOfSegments()
while not rospy.is_shutdown():
    q = np.random.uniform(-0.3, 0.3, 7)
    print "FK", kinematics.FK_vanilla(q, 11)
    J = kinematics.jacobian_vanilla(q)
    print "Jacobian", J
    rospy.sleep(0.2)