Esempio n. 1
0
    def __init__(self, env, use_mocap_ctrl=True, **kwargs):
        super(YumiReachAgent, self).__init__(env, **kwargs)
        import PyKDL
        from gym.utils.kdl_parser import kdl_tree_from_urdf_model
        assert isinstance(env.unwrapped, YumiEnv)
        assert env.unwrapped.block_gripper

        self._env = env
        self._raw_env = env.unwrapped  # type: YumiEnv
        self._sim = self._raw_env.sim
        self._dt = env.unwrapped.dt
        self._goal = None
        self._robot = YumiEnv.get_urdf_model()
        self._kdl = PyKDL
        self.use_mocap_ctrl = use_mocap_ctrl

        # make sure the simulator is ready before getting the transforms
        self._sim.forward()
        self._base_offset = self._sim.data.get_body_xpos(
            'yumi_base_link').copy()

        kdl_tree = kdl_tree_from_urdf_model(self._robot)
        base_link = self._robot.get_root()
        ee_arm_chain_r = kdl_tree.getChain(base_link, 'gripper_r_base')
        ee_arm_chain_l = kdl_tree.getChain(base_link, 'gripper_l_base')
        if self._raw_env.has_right_arm:
            self._add_site_to_chain(ee_arm_chain_r, 'gripper_r_center')
        if self._raw_env.has_left_arm:
            self._add_site_to_chain(ee_arm_chain_l, 'gripper_l_center')

        self._ikp_solver_r = PyKDL.ChainIkSolverPos_LMA(ee_arm_chain_r)
        self._ikp_solver_l = PyKDL.ChainIkSolverPos_LMA(ee_arm_chain_l)
        self._fkp_solver_r = PyKDL.ChainFkSolverPos_recursive(ee_arm_chain_r)
        self._fkp_solver_l = PyKDL.ChainFkSolverPos_recursive(ee_arm_chain_l)
Esempio n. 2
0
    def __init__(self,
                 env,
                 use_qp_solver=False,
                 check_joint_limits=False,
                 use_mocap_ctrl=True,
                 **kwargs):
        super(YumiBarAgent, self).__init__(env, **kwargs)
        import PyKDL
        from gym.utils.kdl_parser import kdl_tree_from_urdf_model
        from gym.envs.yumi.yumi_env import YumiEnv
        assert isinstance(env.unwrapped, YumiEnv)
        assert env.unwrapped.has_two_arms
        assert not env.unwrapped.block_gripper

        self._raw_env = env.unwrapped  # type: YumiEnv
        self._sim = self._raw_env.sim
        self._dt = env.unwrapped.dt
        self._goal = None
        self._phase = 0
        self._phase_steps = 0
        self._locked_l_to_r_tf = None
        self._robot = YumiEnv.get_urdf_model()
        self._kdl = PyKDL
        self.use_mocap_ctrl = use_mocap_ctrl
        self.use_qp_solver = use_qp_solver
        self.check_joint_limits = check_joint_limits

        if check_joint_limits and not use_qp_solver:
            raise ValueError(
                'Joint limits can only be checked with the QP solver!')

        # make sure the simulator is ready before getting the transforms
        self._sim.forward()
        self._base_offset = self._sim.data.get_body_xpos(
            'yumi_base_link').copy()

        kdl_tree = kdl_tree_from_urdf_model(self._robot)
        base_link = self._robot.get_root()
        ee_arm_chain_r = kdl_tree.getChain(base_link, 'gripper_r_base')
        ee_arm_chain_l = kdl_tree.getChain(base_link, 'gripper_l_base')
        if self._raw_env.has_right_arm:
            self._add_site_to_chain(ee_arm_chain_r, 'gripper_r_center')
        if self._raw_env.has_left_arm:
            self._add_site_to_chain(ee_arm_chain_l, 'gripper_l_center')

        self._fkp_solver_r = PyKDL.ChainFkSolverPos_recursive(ee_arm_chain_r)
        self._fkp_solver_l = PyKDL.ChainFkSolverPos_recursive(ee_arm_chain_l)

        self._ikp_solver_r = PyKDL.ChainIkSolverPos_LMA(ee_arm_chain_r)
        self._ikp_solver_l = PyKDL.ChainIkSolverPos_LMA(ee_arm_chain_l)

        self._jac_solver_r = PyKDL.ChainJntToJacSolver(ee_arm_chain_r)
        self._jac_solver_l = PyKDL.ChainJntToJacSolver(ee_arm_chain_l)
Esempio n. 3
0
    def __init__(self):

        self.dh_params = [[-0.033, pi / 2, 0.145, pi, -1],
                          [0.155, 0, 0, pi / 2, -1],
                          [0.135, 0, 0, 0.0, -1],
                          [-0.002, pi / 2, 0, -pi / 2, -1],
                          [0, pi, -0.185, -pi, -1]]

        self.joint_offset = [170*pi/180, 65*pi/180, -146*pi/180, 102.5*pi/180, 167.5*pi/180]

        self.joint_limit_min = [-169*pi/180, -65*pi/180, -151*pi/180, -102.5*pi/180, -167.5*pi/180]
        self.joint_limit_max = [169*pi/180, 90*pi/180, 146*pi/180, 102.5*pi/180, 167.5*pi/180]

        # Setup the subscribers for the joint states
        self.subscriber_joint_state_ = rospy.Subscriber('/joint_states', JointState, self.joint_state_callback,
                                                        queue_size=5)
        # TF2 broadcaster
        self.pose_broadcaster = tf2_ros.TransformBroadcaster()

        # PyKDL
        self.kine_chain = PyKDL.Chain()
        self.setup_kdl_chain()
        self.current_joint_position = PyKDL.JntArray(self.kine_chain.getNrOfJoints())
        self.current_pose = PyKDL.Frame()

        self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.kine_chain)
        self.ik_solver = PyKDL.ChainIkSolverPos_LMA(self.kine_chain)
        self.jac_calc = PyKDL.ChainJntToJacSolver(self.kine_chain)
Esempio n. 4
0
    def __init__(self):
        self.leg = kdl.Chain()
        self.leg.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY), kdl.Frame(kdl.Vector(0,0,-50))))
        self.leg.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY), kdl.Frame(kdl.Vector(0,0,-50))))

        self.ik_solver = kdl.ChainIkSolverPos_LMA(self.leg)

	self.alpha1=45
	self.alpha2=-45
	self.beta=self.alpha2-self.alpha1
Esempio n. 5
0
    def ikSolve(self):

        hulk_kdl_tree = kdl_parser_py.urdf.treeFromFile(
            "/home/local/ASURITE/sdsonawa/catkin_ws/src/jackal/urdf/robot.urdf"
        )[1]
        hulk_transform = hulk_kdl_tree.getChain("dummy_link",
                                                "fake_end_effector_link")
        print("Finding Transform Between base_link and end_effector_link")
        kdl_fk = KDL.ChainFkSolverPos_recursive(hulk_transform)
        kdl_ik = KDL.ChainIkSolverPos_LMA(hulk_transform)
        kdl_input = KDL.JntArray(6)
        kdl_init = KDL.JntArray(6)
        for i in range(5):
            kdl_init[i] = self.joints[0, i]
        kdl_output = KDL.Frame()

        ########################################
        ## offset below provide static transformation between link_0 and camera_link
        x_offset = 0.334
        y_offset = 0.16
        z_offset = 0.59 - 0.35
        ########################################
        initial = True
        # if (initial):
        # kdl_output.p[0] = 0 + x_offset
        # kdl_output.p[1] = -2 + y_offset
        # kdl_output.p[2] = 0 + z_offset
        # initial = False

        # kdl_output.p[0] = self.truss_pose.pose.position.z + x_offset
        # kdl_output.p[1] = -self.truss_pose.pose.position.x - y_offset
        # kdl_output.p[2] =  z_offset  - self.truss_pose.pose.position.y

        kdl_output.p[0] = 0.3
        kdl_output.p[1] = 0.3
        kdl_output.p[2] = 0.0

        t_0 = rospy.get_time()
        kdl_ik.CartToJnt(kdl_init, kdl_output, kdl_input)
        t_1 = rospy.get_time()
        delta = t_1 - t_0
        print(kdl_input[0])
        print(kdl_input[1])
        print(kdl_input[2])
        print(kdl_input[3])
        print(kdl_input[4])

        self.joint1_pub.publish(self.state_output(kdl_input[0]))
        self.joint2_pub.publish(self.state_output(kdl_input[1]))
        self.joint3_pub.publish(self.state_output(kdl_input[2]))
        self.joint4_pub.publish(self.state_output(kdl_input[3]))
        self.joint5_pub.publish(self.state_output(kdl_input[4]))
        self.joint6_pub.publish(self.state_output(kdl_input[5]))
Esempio n. 6
0
    def __init__(self):
        super(YoubotKDL, self).__init__()

        # PyKDL
        self.kine_chain = PyKDL.Chain()
        self.setup_kdl_chain()
        self.current_joint_position = PyKDL.JntArray(
            self.kine_chain.getNrOfJoints())
        self.current_pose = PyKDL.Frame()

        self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.kine_chain)
        self.ik_solver = PyKDL.ChainIkSolverPos_LMA(self.kine_chain)
        self.jac_calc = PyKDL.ChainJntToJacSolver(self.kine_chain)
Esempio n. 7
0
    def __init__(self):
        self.arm = kdl.Chain()
        self.arm.addSegment(
            kdl.Segment(kdl.Joint(kdl.Joint.RotZ),
                        kdl.Frame(kdl.Vector(300, 0, 0))))
        self.arm.addSegment(
            kdl.Segment(kdl.Joint(kdl.Joint.RotZ),
                        kdl.Frame(kdl.Vector(0, -200, 0))))

        self.ik_solver = kdl.ChainIkSolverPos_LMA(self.arm)

        self.current_joints = kdl.JntArray(self.arm.getNrOfJoints())
        self.result_joints = kdl.JntArray(self.arm.getNrOfJoints())
Esempio n. 8
0
    def __init__(self):

        self.base_link = 'base_link'
        self.ee_link = 'ee_link'
        flag, self.tree = kdl_parser.treeFromParam('/robot_description')
        self.chain = self.tree.getChain(self.base_link, self.ee_link)
        self.num_joints = self.tree.getNrOfJoints()
        self.vel_ik_solver = kdl.ChainIkSolverVel_pinv(self.chain)
        self.pos_fk_solver = kdl.ChainFkSolverPos_recursive(self.chain)
        self.pos_ik_solver = kdl.ChainIkSolverPos_LMA(self.chain)

        self.cam_model = image_geometry.PinholeCameraModel()

        self.joint_state = kdl.JntArrayVel(self.num_joints)
        self.joint_names = [
            'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
            'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
        ]

        rospy.init_node('ur_eef_vel_controller')
        rospy.Subscriber('/joint_states', JointState, self.arm_joint_state_cb)
        rospy.Subscriber('/rdda_interface/joint_states', JointState,
                         self.finger_joint_state_cb)
        self.joint_vel_cmd_pub = rospy.Publisher(
            '/joint_group_vel_controller/command',
            Float64MultiArray,
            queue_size=10)
        self.joint_pos_cmd_pub = rospy.Publisher(
            '/scaled_pos_traj_controller/command',
            JointTrajectory,
            queue_size=10)
        self.speed_scaling_pub = rospy.Publisher('/speed_scaling_factor',
                                                 Float64,
                                                 queue_size=10)

        self.switch_controller_cli = rospy.ServiceProxy(
            '/controller_manager/switch_controller', SwitchController)
        self.joint_pos_cli = actionlib.SimpleActionClient(
            '/scaled_pos_joint_traj_controller/follow_joint_trajectory',
            FollowJointTrajectoryAction)

        cam_info = rospy.wait_for_message('/camera/depth/camera_info',
                                          CameraInfo,
                                          timeout=10)
        self.cam_model.fromCameraInfo(cam_info)

        self.bridge = CvBridge()  # allows conversion from depth image to array
        self.tf_listener = tf.TransformListener()
        self.image_offset = 100
        self.pos_ctrler_running = False
        rospy.sleep(0.5)
 def initialize_kinematic_solvers(self):
     robot_description = rospy.get_param('/yumi/velocity_control/robot_description', '/robot_description')
     self._robot = URDF.from_parameter_server(key=robot_description)
     self._kdl_tree = kdl_tree_from_urdf_model(self._robot)
     # self._base_link = self._robot.get_root()
     self._ee_link = 'gripper_' +self._arm_name + '_base' #name of the frame we want
     self._ee_frame = PyKDL.Frame()
     self._ee_arm_chain = self._kdl_tree.getChain(self._base_link, self._ee_link)
     # KDL Solvers
     self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._ee_arm_chain)
     self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._ee_arm_chain)
     self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._ee_arm_chain)
     #self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl)
     self._ik_p_kdl = PyKDL.ChainIkSolverPos_LMA(self._ee_arm_chain)
     self._jac_kdl = PyKDL.ChainJntToJacSolver(self._ee_arm_chain)
     self._dyn_kdl = PyKDL.ChainDynParam(self._ee_arm_chain, PyKDL.Vector.Zero())
def generate_solver(urdf_str: str):
    """Create an FK/IK solvers for each arm (left/right)."""
    success, urdf_tree = urdf.treeFromString(urdf_str)
    if not success:
        raise IOError('Could not parse the URDF!')

    chains = {}
    fk_solvers = {}
    ik_solvers = {}

    for side in ['left', 'right']:
        chains[side] = urdf_tree.getChain('torso', f'{side}_tip')
        fk_solvers[side] = kdl.ChainFkSolverPos_recursive(chains[side])

        ik_solvers[side] = kdl.ChainIkSolverPos_LMA(
            chains[side],
            eps=1e-5,
            _maxiter=500,
            _eps_joints=1e-15,
        )

    return chains, fk_solvers, ik_solvers
Esempio n. 11
0
    kdl_model = kdl_model[1]
else:
    raise ValueError("Error during the parsing")

#####################
# Chain, FK, and IK #
#####################

#define the kinematic chain
chain_RHand = kdl_model.getChain('DWYTorso', 'RForearm')

#define the Forward Kinematic solver
FK_RHand = kdl.ChainFkSolverPos_recursive(chain_RHand)

#define the Inverse Kinematic solver
IK_RHand = kdl.ChainIkSolverPos_LMA(chain_RHand)

##############
# Gazebo/ROS #
##############

#Define arm
Rarm_joints = ['RShSag', 'RShLat', 'RShYaw', 'RElbj', 'RForearmPlate']

#create publisher
if 'publisher' not in globals():
    publisher = ComanPublisher()

#define ROS rate
r = rospy.Rate(ros_rate)
def main():
    rospy.init_node('main_test', anonymous=True)
    rospy.Subscriber("/iiwa/joint_states", JointState, iiwaStateCallback)

    # Initialize publishers for send computation torques.
    for i in range(0,7):
        torque_controller_pub.append(rospy.Publisher("/iiwa/joint" + str(i+1) + "_torque_controller/command", Float64, queue_size=10))

    rate = rospy.Rate(int(1.0/dt))

    # Get parameters for kinematic from setup.yaml file
    base_link = rospy.get_param("/master/base_link_name")
    tool_link = rospy.get_param("/master/tool_link_name")
    urdf = rospy.get_param("iiwa_urdf_model")

    rospy.loginfo("tip_name:  %s" % tool_link)
    rospy.loginfo("root_name: %s" % base_link)

    data = json.load(open("/workspace/base_ws/src/profi2021_metrics_graber/data/master_tests/track_1_traj.json", "r"))

    # Generate kinematic model for orocos_kdl
    (ok, tree) = kdl_parser_py.urdf.treeFromString(urdf)
    chain = tree.getChain(base_link, tool_link)
    L = np.transpose(np.array([[1, 1, 1, 0.01, 0.01, 0.01]]))
    iksolverpos = kdl.ChainIkSolverPos_LMA(chain, L)

    # Generate dynamic model for orocos_kdl
    grav = kdl.Vector(0, 0, -9.82)
    dyn_model = kdl.ChainDynParam(chain, grav)

    startControllers()

    torq_msg = Float64()
    counter = 0
    u = kdl.JntArray(7)
    q_dest = kdl.JntArray(7)
    frame_dest = kdl.Frame()

    # Setting up initial point


    frame_dest.p[0] = data[0]["x"]
    frame_dest.p[1] = data[0]["y"]
    frame_dest.p[2] = 0.3
    # frame_dest.p[0] = 0.3
    # frame_dest.p[1] = 0.0
    # frame_dest.p[2] = 0.05
    frame_dest.M = kdl.Rotation.RotY(3.14)

    ret = iksolverpos.CartToJnt(q, frame_dest, q_dest)

    while not rospy.is_shutdown():
        frame_dest.p[0] = data[int(counter/10)]["x"]
        frame_dest.p[1] = data[int(counter/10)]["y"];    # Start controllers
        if counter>89*10:
            counter=0
        ret = iksolverpos.CartToJnt(q, frame_dest, q_dest)
        rospy.loginfo("IK solution: %f, %f, %f, %f, %f, %f, %f", q_dest[0],  q_dest[1],  q_dest[2],  q_dest[3],  q_dest[4],  q_dest[5],  q_dest[6])
        dyn_model.JntToGravity(q, grav_torques)
        rospy.loginfo("Estimation torque on joins: %f, %f, %f, %f, %f, %f, %f\n" % (grav_torques[0], grav_torques[1],grav_torques[2], grav_torques[3], grav_torques[4], grav_torques[5], grav_torques[6]))
        for i in range(0,7):
            u[i] = KP[i]*(q_dest[i]-q[i]) - KD[i]*dq[i] + grav_torques[i]
            torq_msg.data = u[i]
            torque_controller_pub[i].publish(torq_msg)
        counter+=1
        rate.sleep()
Esempio n. 13
0
    def init(self, _chain):
        kdlChain = kdl.Chain()

        body = _chain.getBaseBody()

        # Joint Limits
        self.q_min = kdl.JntArray(len(_chain.getJoints()))
        self.q_max = kdl.JntArray(len(_chain.getJoints()))

        old_pos = kdl.Vector(0, 0, 0)

        index = 0
        while (len(body.child_joints) != 0):  # Last link
            # Error if this robot is not a serial robot.
            if len(body.child_joints) > 1:
                raise InvalidChainError()

            # Get Joint Object of type Solver.Joint
            joint = _chain.getJoint(body.child_joints[0])

            # From Joint (Location of joint on parent body)
            trans = kdl.Vector(float(joint.parent_pivot['x']),
                               float(joint.parent_pivot['y']),
                               float(joint.parent_pivot['z']))

            # From Joint (Location of joint on child body)
            trans2 = kdl.Vector(float(joint.child_pivot['x']),
                                float(joint.child_pivot['y']),
                                float(joint.child_pivot['z']))

            frame = kdl.Frame(trans + old_pos)
            old_pos = trans2

            kdlJoint = kdl.Joint()  # Default Joint Axis

            if joint.parent_axis['x']:
                kdlJoint = kdl.Joint(kdl.Joint.RotX,
                                     scale=joint.parent_axis['x'])
            elif joint.parent_axis['y']:
                kdlJoint = kdl.Joint(kdl.Joint.RotY,
                                     scale=joint.parent_axis['y'])
            elif joint.parent_axis['z']:
                kdlJoint = kdl.Joint(kdl.Joint.RotZ,
                                     scale=joint.parent_axis['z'])

            # Add to KDL
            kdlChain.addSegment(kdl.Segment(kdlJoint, frame))

            # Joint limits
            self.q_min[index] = joint.joint_limits['low']
            self.q_max[index] = joint.joint_limits['high']

            body = _chain.getBody(body.children[0])
            index += 1

        self.kdlChain = kdlChain

        # Initialize Solvers
        self.FKSolverPos = kdl.ChainFkSolverPos_recursive(self.kdlChain)
        # self.IKVelSolver = kdl.ChainIkSolverVel_pinv_givens(self.kdlChain)
        # self.IKPosSolver = kdl.ChainIkSolverPos_NR_JL(self.kdlChain, self.q_min, self.q_max,
        #   self.FKSolverPos, self.IKVelSolver)

        self.IKPosSolver = kdl.ChainIkSolverPos_LMA(self.kdlChain)
Esempio n. 14
0
jointParameters = kdl.JntArray(chain.getNrOfJoints())
jointParameters[0] = -0.57
jointParameters[1] = 0.12
jointParameters[2] = 0.75
jointParameters[3] = 0.71
# jointParameters[4] = 0
# jointParameters[5] = 0.785
# jointParameters[6] = -0.06

FKSolver = kdl.ChainFkSolverPos_recursive(chain)
FKSolver.JntToCart(jointParameters, output_frame)

print("Final frame :---")
print(output_frame)

IKPosSolver = kdl.ChainIkSolverPos_LMA(chain)

desired_q = kdl.JntArray(chain.getNrOfJoints())
zero_q = kdl.JntArray(chain.getNrOfJoints())

tip_frame = output_frame  #kdl.Frame()

IKPosSolver.CartToJnt(zero_q, tip_frame, desired_q)
print("desired_q:--")
print(desired_q)

print("-------------------------")
print("Diff ...")
for i in range(chain.getNrOfJoints()):
    print("%3.3f" % (jointParameters[i] - desired_q[i]))
    if model[0]:
        model = model[1]
    else:
        raise ValueError("Error during the parsing")

    # define the kinematic chain
    chain = model.getChain(base_name, end_effector_name)
    print("Number of joints in the chain: {}".format(chain.getNrOfJoints()))

    # define the FK solver
    FK = kdl.ChainFkSolverPos_recursive(chain)

    # define the IK Solver
    # IKV = kdl.ChainIkSolverVel_pinv(chain)
    # IK = kdl.ChainIkSolverPos_NR(chain, FK, IKV)
    IK = kdl.ChainIkSolverPos_LMA(
        chain)  # ,_maxiter=ik_max_iter, _eps_joints=ik_tol)

    # desired final cartesian position
    Fd = kdl.Frame(kdl.Vector(xd[0], xd[1], xd[2]))

    for _ in count():
        # get current position in the task/operational space
        # x = robot.get_link_positions(link_id, wrt_link_id)
        x = robot.get_link_world_positions(link_id)
        print("(xd - x) = {}".format(xd - x))

        # buffer to put the solution
        q_solved = kdl.JntArray(chain.getNrOfJoints())

        # initial joint positions
        q = robot.get_joint_positions(joint_ids)