Пример #1
0
 def set_jep(self, arm, q, duration=None):
     if arm == 'right_arm': 
         pub = self.r_jep_cmd_pub
     elif arm == 'left_arm':
         pub = self.l_jep_cmd_pub
     else:
         raise RuntimeError('Undefined arm: %s'%(arm))
     time_stamp = rospy.Time.now()
     h = Header()
     h.stamp = time_stamp
     pub.publish(FloatArray(h, q))
Пример #2
0
 def get_new_path(self):
   from nav_msgs.msg import Path
   from roslib.msg import Header
   from geometry_msgs.msg import PoseStamped
   from geometry_msgs.msg import Pose
   from geometry_msgs.msg import Point
   from geometry_msgs.msg import Quaternion
   
   return Path(None, [PoseStamped(Header(0,rospy.Time(0,0),'foo'),    Pose(Point(1.23, 4.56, 7.89), Quaternion(1,0,0,1))),
                      PoseStamped(Header(0,rospy.Time(0,1000),'foo'), Pose(Point(1.25, 4.58, 7.91), Quaternion(0,1,0,1))),
                      PoseStamped(Header(0,rospy.Time(0,2000),'foo'), Pose(Point(1.27, 4.60, 7.93), Quaternion(0,0,1,1)))])
Пример #3
0
 def set_impedance_scale(self, arm, s):
     if arm == 'right_arm': 
         pub = self.r_alph_cmd_pub
     elif arm == 'left_arm':
         pub = self.l_alph_cmd_pub
     else:
         raise RuntimeError('Undefined arm: %s'%(arm))
     time_stamp = rospy.Time.now()
     h = Header()
     h.stamp = time_stamp
     pub.publish(FloatArray(h, s))
Пример #4
0
 def set_jep(self, arm, q, duration=None):
     if arm == 'right_arm':
         pub = self.r_jep_cmd_pub
     elif arm == 'left_arm':
         pub = self.l_jep_cmd_pub
     else:
         raise RuntimeError('Undefined arm: %s' % (arm))
     time_stamp = rospy.Time.now()
     h = Header()
     h.stamp = time_stamp
     pub.publish(FloatArray(h, q))
Пример #5
0
 def set_impedance_scale(self, arm, s):
     if arm == 'right_arm':
         pub = self.r_alph_cmd_pub
     elif arm == 'left_arm':
         pub = self.l_alph_cmd_pub
     else:
         raise RuntimeError('Undefined arm: %s' % (arm))
     time_stamp = rospy.Time.now()
     h = Header()
     h.stamp = time_stamp
     pub.publish(FloatArray(h, s))
Пример #6
0
 def get_new_occupancy_grid(self):
   from nav_msgs.msg import OccupancyGrid
   from nav_msgs.msg import MapMetaData
   from roslib.msg import Header
   from geometry_msgs.msg import Pose
   from geometry_msgs.msg import Point
   from geometry_msgs.msg import Quaternion
   
   import random
   r = random.Random(1234)
   
   return OccupancyGrid(Header(0, rospy.Time(0,0), "/map"), MapMetaData(rospy.Time(123,456), 0.1, 100, 100, Pose(Point(1.23, 4.56, 7.89), Quaternion(0,0,0,1))), [r.randint(-1,100) for x in xrange(0,1000)])
Пример #7
0
    def init_goal(self, goal, transition_cb=None, feedback_cb=None):
        action_goal = self.ActionGoal(header=Header(),
                                      goal_id=self._generate_id(),
                                      goal=goal)

        csm = CommStateMachine(action_goal, transition_cb, feedback_cb,
                               self.send_goal_fn, self.cancel_fn)

        self.list_mutex.acquire()
        try:
            self.statuses.append(weakref.ref(csm))
        finally:
            self.list_mutex.release()

        self.send_goal_fn(action_goal)

        return GoalHandle(csm)
Пример #8
0
 def get_new_joy(self):
     from sensor_msgs.msg import Joy
     from roslib.msg import Header
     return Joy(Header(), [0.1, 0.2, 0.3, 0.4, 0.5], [0, 1, 0, 1, 0])
Пример #9
0
    def start(self):
        self.proxy.start()

        self.ndof_finger = 3

        self.flex_factor_index = [0.3] * self.ndof_finger
        self.flex_factor_ring = [0.3] * self.ndof_finger
        self.flex_factor_pinky = [0.3] * self.ndof_finger
        self.flex_factor_thumb = [0.3] * 2

        self.p = subprocess.Popen(
            ['roslaunch', 'm3_defs_ros', 'm3_launch.launch'])
        rospy.init_node("joint_state_publisher")
        self.pub = rospy.Publisher("/joint_states", JointState)
        time.sleep(4.0)

        hand_names = self.proxy.get_available_components('m3hand')

        self.hands = []
        self.hand_nums = []

        for i in range(len(hand_names)):
            self.hands.append(m3f.create_component(hand_names[i]))
            self.proxy.subscribe_status(self.hands[i])
            #self.proxy.publish_command(self.hands[i])
            if hand_names[i][-2].isdigit():
                self.hand_nums.append(hand_names[i][-2:])
            else:
                self.hand_nums.append(hand_names[i][-1])

#r_hand_ua_num = 14

        self.ndof_hand_ua = 12

        self.positions = []
        self.joints = []

        for j in range(len(self.hands)):
            for i in range(self.ndof_hand_ua):
                self.positions.append(0.0)
                self.joints.append('m3joint_ua_mh' + str(self.hand_nums[j]) +
                                   '_j' + str(i))

# Thumb: J0,J1,J2
# Index: J3, J4, J5
# Ring: J6,J7,J8
# Pinkie: J9, J10, J11

        print 'Starting hand viz.'

        while (True):
            self.positions = []
            self.proxy.step()
            for i in range(len(self.hands)):
                th = self.hands[i].get_theta_rad()

                #Thumb
                self.positions.append(-th[0] + 1.57)  #0
                self.positions.append(th[1] * self.flex_factor_thumb[0])
                self.positions.append(th[1] * self.flex_factor_thumb[1])
                #Index
                self.positions.append(th[2] * self.flex_factor_index[0])
                self.positions.append(th[2] * self.flex_factor_index[1])
                self.positions.append(th[2] * self.flex_factor_index[2])
                #Ring
                self.positions.append(th[3] * self.flex_factor_ring[0])
                self.positions.append(th[3] * self.flex_factor_ring[1])
                self.positions.append(th[3] * self.flex_factor_ring[2])
                #Pinkie
                self.positions.append(th[4] * self.flex_factor_pinky[0])
                self.positions.append(th[4] * self.flex_factor_pinky[1])
                self.positions.append(th[4] * self.flex_factor_pinky[2])

            if self.pub is not None and not rospy.is_shutdown():
                header = Header(0, rospy.Time.now(), '0')
                self.pub.publish(
                    JointState(header, self.joints, self.positions,
                               [0] * len(self.positions),
                               [0] * len(self.positions)))
            else:
                print 'Error...exiting.'
                break
            time.sleep(0.1)
Пример #10
0
    def step_ros(self):
        r_arm = 'right_arm'
        l_arm = 'left_arm'

        self.cb_lock.acquire()
        r_jep = copy.copy(self.r_jep)
        l_jep = copy.copy(self.l_jep)
        r_alpha = copy.copy(self.arm_settings['right_arm'].stiffness_list)
        l_alpha = copy.copy(self.arm_settings['left_arm'].stiffness_list)
        self.cb_lock.release()

        self.set_jep(r_arm, r_jep)
        self.set_jep(l_arm, l_jep)
        self.set_alpha(r_arm, r_alpha)
        self.set_alpha(l_arm, l_alpha)

        self.step()

        motor_pwr_state = self.is_motor_power_on()

        if not motor_pwr_state:
            self.maintain_configuration()

        q_r = self.get_joint_angles(r_arm)
        q_l = self.get_joint_angles(l_arm)

        if self.floating_arms:
            if self.qr_prev == None or self.floating_arms_counter < 100:
                self.qr_prev = q_r
                self.ql_prev = q_l
                self.floating_arms_counter += 1
            else:
                tol = np.radians([5., 2., 10., 2., 10., 0.03, 0.6])
                r_arr = np.array(q_r)
                l_arr = np.array(q_l)
                prev_r_arr = np.array(self.qr_prev)
                prev_l_arr = np.array(self.ql_prev)

                dqr = np.array(q_r) - np.array(prev_r_arr)
                dql = np.array(q_l) - np.array(prev_l_arr)
                dqr = dqr * (np.abs(dqr) > tol)
                dql = dql * (np.abs(dql) > tol)

                r_jep = (np.array(r_jep) + dqr).tolist()
                l_jep = (np.array(l_jep) + dql).tolist()

                self.cb_lock.acquire()
                self.r_jep = copy.copy(r_jep)
                self.l_jep = copy.copy(l_jep)
                self.cb_lock.release()

                change_idxs = np.where(dqr != 0)
                prev_r_arr[change_idxs] = r_arr[change_idxs]
                change_idxs = np.where(dql != 0)
                prev_l_arr[change_idxs] = l_arr[change_idxs]
                self.qr_prev = prev_r_arr.tolist()
                self.ql_prev = prev_l_arr.tolist()

        f_raw_r = self.get_wrist_force(r_arm).A1.tolist()
        f_raw_l = self.get_wrist_force(l_arm).A1.tolist()
        f_r = self.xhat_force[r_arm]
        f_l = self.xhat_force[l_arm]

        # publish stuff over ROS.
        time_stamp = rospy.Time.now()
        h = Header()
        h.stamp = time_stamp

        self.q_r_pub.publish(FloatArray(h, q_r))
        self.q_l_pub.publish(FloatArray(h, q_l))

        self.jep_r_pub.publish(FloatArray(h, r_jep))
        self.jep_l_pub.publish(FloatArray(h, l_jep))

        self.alph_r_pub.publish(FloatArray(h, r_alpha))
        self.alph_l_pub.publish(FloatArray(h, l_alpha))

        h.frame_id = '/torso_lift_link'
        nms = self.joint_names_list['right_arm'] + self.joint_names_list[
            'left_arm']
        pos = q_r + q_l
        self.joint_state_pub.publish(
            JointState(h, nms, pos, [0.] * len(pos), [0.] * len(pos)))

        h.frame_id = ar.link_tf_name(r_arm, 7)
        self.force_raw_r_pub.publish(FloatArray(h, f_raw_r))
        self.force_r_pub.publish(FloatArray(h, f_r))

        h.frame_id = ar.link_tf_name(l_arm, 7)
        self.force_raw_l_pub.publish(FloatArray(h, f_raw_l))
        self.force_l_pub.publish(FloatArray(h, f_l))

        self.pwr_state_pub.publish(Bool(motor_pwr_state))
Пример #11
0
    rospy.logout('Sleeping ...')
    rospy.sleep(1.0)
    rospy.logout('... begin')

    r_arm = 'right_arm'
    l_arm = 'left_arm'

    transform_bcast = tfb.TransformBroadcaster()
    torso_link_name = ar.link_tf_name(r_arm, 0)
    while not rospy.is_shutdown():
        rospy.sleep(0.1)
        f_r = arm_client.get_wrist_force(r_arm, base_frame=True)
        f_l = arm_client.get_wrist_force(l_arm, base_frame=True)
        time_stamp = rospy.Time.now()
        h = Header()
        h.stamp = time_stamp
        force_r_pub.publish(FloatArray(h, f_r))
        force_l_pub.publish(FloatArray(h, f_l))

        publish_sphere_marker((0.5,0.5,0.5), 0.08, torso_link_name,
                              time_stamp, 'both_arms', 0)

        for arm in [r_arm, l_arm]:
            q = arm_client.get_joint_angles(arm)
            links = [2, 3, 7]
            for i in links:
                p, rot = arms.FK_all(arm, q, i)
                qaut = tr.matrix_to_quaternion(rot)
                frameid = ar.link_tf_name(arm, i)
                transform_bcast.sendTransform(p.A1.tolist(), qaut, time_stamp,
Пример #12
0
from manipulation_transforms.srv import *
from roslib.msg import Header

if __name__ == "__main__":

    rospy.init_node(PKG)
    ns = "/cart_pushing/"
    #ns = ""
    service_name = ns + PKG + "_server/SetInitialTransforms"
    rospy.loginfo("Waiting for service \"%s\"" % service_name)
    rospy.wait_for_service(service_name)

    try:
        SetInitialTransforms_srv = rospy.ServiceProxy(service_name,
                                                      SetInitialTransforms)
        obj_init_pose = PoseStamped(header=Header(frame_id="base_footprint",
                                                  stamp=rospy.Time.now()),
                                    pose=Pose(position=Point(0.5, 0.0, 0.344),
                                              orientation=Quaternion(
                                                  -0.5, -0.44, -0.45, 0.59)))

        gripper_init_poses = []
        gripper_init_poses.append(
            PoseStamped(header=Header(frame_id="base_footprint",
                                      stamp=rospy.Time.now()),
                        pose=Pose(position=Point(0.5, -0.12, 0.344),
                                  orientation=Quaternion(
                                      -0.5, -0.44, -0.45, 0.59))))

        gripper_init_poses.append(
            PoseStamped(header=Header(frame_id="base_footprint",
                                      stamp=rospy.Time.now()),
Пример #13
0
    def step_ros(self):
        r_arm = 'right_arm'
        l_arm = 'left_arm'

        self.cb_lock.acquire()
        r_jep = copy.copy(self.r_jep)
        l_jep = copy.copy(self.l_jep)
        r_alpha = copy.copy(self.arm_settings['right_arm'].stiffness_list)
        l_alpha = copy.copy(self.arm_settings['left_arm'].stiffness_list)
        self.cb_lock.release()

        self.set_jep(r_arm, r_jep)
        self.set_jep(l_arm, l_jep)
        self.set_alpha(r_arm, r_alpha)
        self.set_alpha(l_arm, l_alpha)

        self.step()

        motor_pwr_state = self.is_motor_power_on()

        if not motor_pwr_state:
            self.maintain_configuration()

        q_r = self.get_joint_angles(r_arm)
        q_l = self.get_joint_angles(l_arm)

        if self.floating_arms:
            if self.qr_prev == None or self.floating_arms_counter < 100:
                self.qr_prev = q_r
                self.ql_prev = q_l
                self.floating_arms_counter += 1
            else:
                tol = np.radians([5., 2., 10., 2., 10., 0.03, 0.6])
                r_arr = np.array(q_r)
                l_arr = np.array(q_l)
                prev_r_arr = np.array(self.qr_prev)
                prev_l_arr = np.array(self.ql_prev)

                dqr = np.array(q_r) - np.array(prev_r_arr)
                dql = np.array(q_l) - np.array(prev_l_arr)
                dqr = dqr * (np.abs(dqr) > tol)
                dql = dql * (np.abs(dql) > tol)
                
                r_jep = (np.array(r_jep) + dqr).tolist()
                l_jep = (np.array(l_jep) + dql).tolist()

                self.cb_lock.acquire()
                self.r_jep = copy.copy(r_jep)
                self.l_jep = copy.copy(l_jep)
                self.cb_lock.release()

                change_idxs = np.where(dqr != 0)
                prev_r_arr[change_idxs] = r_arr[change_idxs]
                change_idxs = np.where(dql != 0)
                prev_l_arr[change_idxs] = l_arr[change_idxs]
                self.qr_prev = prev_r_arr.tolist()
                self.ql_prev = prev_l_arr.tolist()

        f_raw_r = self.get_wrist_force(r_arm).A1.tolist()
        f_raw_l = self.get_wrist_force(l_arm).A1.tolist()
        f_r = self.xhat_force[r_arm]
        f_l = self.xhat_force[l_arm]

        # publish stuff over ROS.
        time_stamp = rospy.Time.now()
        h = Header()
        h.stamp = time_stamp

        self.q_r_pub.publish(FloatArray(h, q_r))
        self.q_l_pub.publish(FloatArray(h, q_l))

        self.jep_r_pub.publish(FloatArray(h, r_jep))
        self.jep_l_pub.publish(FloatArray(h, l_jep))

        self.alph_r_pub.publish(FloatArray(h, r_alpha))
        self.alph_l_pub.publish(FloatArray(h, l_alpha))

        h.frame_id = '/torso_lift_link'
        nms = self.joint_names_list['right_arm'] + self.joint_names_list['left_arm']
        pos = q_r + q_l
        self.joint_state_pub.publish(JointState(h, nms, pos,
                                    [0.]*len(pos), [0.]*len(pos)))

        h.frame_id = ar.link_tf_name(r_arm, 7)
        self.force_raw_r_pub.publish(FloatArray(h, f_raw_r))
        self.force_r_pub.publish(FloatArray(h, f_r))
        
        h.frame_id = ar.link_tf_name(l_arm, 7)
        self.force_raw_l_pub.publish(FloatArray(h, f_raw_l))
        self.force_l_pub.publish(FloatArray(h, f_l))

        self.pwr_state_pub.publish(Bool(motor_pwr_state))