Python mirror_arm_joints Examples

Example #1

File: reset.py Project: ankush-me/sandbox444

#!/usr/bin/env python

import roslib
roslib.load_manifest('rospy')
import rospy

import utils.conversions as conv
from brett2.PR2 import PR2
from brett2.PR2 import mirror_arm_joints
import math   
import numpy
 


if __name__=='__main__':
    rospy.init_node('ankush')
    bot = PR2()

    bot.lgrip.open()
    bot.rgrip.open()
    fold_reset = [math.pi/32,  math.pi/12, 0,
                  -math.pi/2,  math.pi ,  -math.pi/6,  -math.pi/4]    
    bot.larm.goto_joint_positions(fold_reset)
    bot.rarm.goto_joint_positions(mirror_arm_joints(fold_reset))

    rospy.spin()

Example #2

File: multidof_command_vs_actual.py Project: ankush-me/python

from brett2.PR2 import PR2, mirror_arm_joints


JOINT_NAME = "l_gripper_joint"
CMD_TOPIC = "/l_gripper_controller/command"
if rospy.get_name() == "/unnamed":
    rospy.init_node("check_command_vs_actual", disable_signals = True)

pr2 = PR2.create()
pr2.update_rave()
n_steps = 30
cmd_times = np.linspace(0,3,n_steps)
cmd_states = np.zeros(n_steps, dtype=trajectories.BodyState)

r_arm_cur = pr2.rarm.get_joint_positions()
cmd_states["r_arm"] = mu.linspace2d(r_arm_cur, mirror_arm_joints(pr2.rarm.L_POSTURES["untucked"]) - r_arm_cur,n_steps)
l_arm_cur = pr2.larm.get_joint_positions()
cmd_states["l_arm"] = mu.linspace2d(l_arm_cur, pr2.rarm.L_POSTURES["tucked"] - l_arm_cur, n_steps)
r_grip_cur = pr2.rgrip.get_angle()
cmd_states["r_gripper"] = np.linspace(r_grip_cur, .08 - r_grip_cur, n_steps) 
l_grip_cur = pr2.lgrip.get_angle()
cmd_states["l_gripper"] = np.linspace(l_grip_cur, .08 - l_grip_cur, n_steps) 
base_cur = pr2.base.get_pose("/odom_combined")
cmd_states["base"] = mu.linspace2d(base_cur, np.random.randn(3), n_steps)


trajectories.follow_body_traj(pr2, cmd_states, times=cmd_times, r_arm=True, l_arm=True, r_gripper=True, l_gripper=True, base=True, wait=False)


actual_times = []
actual_states = np.zeros(5000, trajectories.BodyState)

Example #3

JOINT_NAME = "l_gripper_joint"
CMD_TOPIC = "/l_gripper_controller/command"
if rospy.get_name() == "/unnamed":
    rospy.init_node("check_command_vs_actual", disable_signals=True)

pr2 = PR2.create()
pr2.update_rave()
n_steps = 30
cmd_times = np.linspace(0, 3, n_steps)
cmd_states = np.zeros(n_steps, dtype=trajectories.BodyState)

r_arm_cur = pr2.rarm.get_joint_positions()
cmd_states["r_arm"] = mu.linspace2d(
    r_arm_cur,
    mirror_arm_joints(pr2.rarm.L_POSTURES["untucked"]) - r_arm_cur, n_steps)
l_arm_cur = pr2.larm.get_joint_positions()
cmd_states["l_arm"] = mu.linspace2d(l_arm_cur,
                                    pr2.rarm.L_POSTURES["tucked"] - l_arm_cur,
                                    n_steps)
r_grip_cur = pr2.rgrip.get_angle()
cmd_states["r_gripper"] = np.linspace(r_grip_cur, .08 - r_grip_cur, n_steps)
l_grip_cur = pr2.lgrip.get_angle()
cmd_states["l_gripper"] = np.linspace(l_grip_cur, .08 - l_grip_cur, n_steps)
base_cur = pr2.base.get_pose("/odom_combined")
cmd_states["base"] = mu.linspace2d(base_cur, np.random.randn(3), n_steps)

trajectories.follow_body_traj(pr2,
                              cmd_states,
                              times=cmd_times,
                              r_arm=True,

Example #4

#!/usr/bin/env python

import roslib

roslib.load_manifest('rospy')
import rospy

import utils.conversions as conv
from brett2.PR2 import PR2
from brett2.PR2 import mirror_arm_joints
import math
import numpy

if __name__ == '__main__':
    rospy.init_node('ankush')
    bot = PR2()

    bot.lgrip.open()
    bot.rgrip.open()
    fold_reset = [
        math.pi / 32, math.pi / 12, 0, -math.pi / 2, math.pi, -math.pi / 6,
        -math.pi / 4
    ]
    bot.larm.goto_joint_positions(fold_reset)
    bot.rarm.goto_joint_positions(mirror_arm_joints(fold_reset))

    rospy.spin()