Exemple #1
0
def rotary_mechanism_configurations_from_global_poses(pts_2d, center):
    st_pt = pts_2d[:,0]          
    cx = center[0,0]
    cy = center[1,0]
    start_angle = tr.angle_within_mod180(math.atan2(st_pt[1,0]-cy, st_pt[0,0]-cx) - math.radians(90)) #-90 for display
    poses_list = []        
    for pos_idx in range(pts_2d.shape[1]):
        end_pt = pts_2d[:, pos_idx]
        end_angle = tr.angle_within_mod180(math.atan2(end_pt[1,0]-cy, end_pt[0,0]-cx) - math.radians(90))                        
        poses_list.append(end_angle)
    return poses_list
Exemple #2
0
def rotary_mechanism_configurations_from_global_poses(pts_2d, center):
    st_pt = pts_2d[:, 0]
    cx = center[0, 0]
    cy = center[1, 0]
    start_angle = tr.angle_within_mod180(
        math.atan2(st_pt[1, 0] - cy, st_pt[0, 0] - cx) -
        math.radians(90))  #-90 for display
    poses_list = []
    for pos_idx in range(pts_2d.shape[1]):
        end_pt = pts_2d[:, pos_idx]
        end_angle = tr.angle_within_mod180(
            math.atan2(end_pt[1, 0] - cy, end_pt[0, 0] - cx) -
            math.radians(90))
        poses_list.append(end_angle)
    return poses_list
    def update_eq_point(self, arm, motion_vec, step_size, rot_mat):
        x,y,a,dx,dy,da = 0.,0.,0.,0.,0.,0.
        st = self.segway_trajectory
        if len(st.x_list)>0:
            x,y,a = st.x_list[-1],st.y_list[-1],st.a_list[-1]
        if len(st.x_list)>1:
            dx = x-st.x_list[-2]
            dy = y-st.y_list[-2]
            da = tr.angle_within_mod180(x-st.a_list[-2])

        self.eq_pt_cartesian = smc.tlTts(self.eq_pt_cartesian_ts,x,y,a)
        if len(self.zenither_list) > 2:
            h = self.zenither_list[-1] - self.zenither_list[-2]
            self.eq_pt_cartesian[2,0] -= h

        next_pt = self.eq_pt_cartesian + step_size * motion_vec

        if self.q_guess != None:
            if arm == 'right_arm':
                self.q_guess[1] = ut.bound(self.q_guess[1],math.radians(20),math.radians(5))

        q_eq = self.firenze.IK(arm,next_pt,rot_mat,self.q_guess)
        self.eq_pt_cartesian = next_pt
        self.eq_pt_cartesian_ts = smc.tsTtl(self.eq_pt_cartesian,x,y,a)
        self.q_guess = q_eq
        return q_eq
    def reposition_robot(self,hook_location,turn_angle,hook_angle,position_number=2):
        h = self.workspace_dict[hook_angle]['height']
        max_z_height = 1.3
        min_z_height = 0.5
        h_desired = self.z.get_position_meters()+hook_location[2,0]-h
        print 'h_desired:',h_desired
        print 'h:',h
        h_achieve = ut.bound(h_desired,max_z_height,min_z_height)
        h_err = h_desired-h_achieve
        h = h+h_err
        print 'h_achieve:',h_achieve

        wkd = self.workspace_dict[hook_angle]['pts']
        k = wkd.keys()
        k_idx = np.argmin(np.abs(np.array(k)-h))
        wrkspc_pts = wkd[k[k_idx]]

#        good_location = np.matrix([0.45,-0.35,h]).T
        good_location = wrkspc_pts.mean(1)
        good_location = np.matrix(good_location.A1.tolist()+[h]).T

        if position_number == 1:
            good_location[0,0] += 0.0
            good_location[1,0] -= 0.1
        elif position_number == 2:
            good_location[0,0] += 0.0
            good_location[1,0] -= 0.0
        elif position_number == 3:
            good_location[0,0] += 0.0
            good_location[1,0] += 0.1
        else:
            good_location[0,0] -= 0.1
            good_location[1,0] -= 0.0

        good_location = mcf.mecanumTglobal(mcf.globalTtorso(good_location))
        hook_location = mcf.mecanumTglobal(mcf.globalTtorso(hook_location))

        v = tr.Rz(-turn_angle)*good_location
        move_vector = hook_location - v

        pose1 = self.segway_pose.get_pose()
        self.segway_command_node.go_xya_pos_local(move_vector[0,0],move_vector[1,0],
                                                  turn_angle,blocking=False)

        self.z.torque_move_position(h_achieve)
        print 'before waiting for segway to stop moving.'
        self.segway_command_node.wait_for_tolerance_pos(0.03,0.03,math.radians(4))
        print 'after segway has stopped moving.'
        
        pose2 = self.segway_pose.get_pose()
        hl_new = vop.transform_pts_local(hook_location[0:2,:],pose1,pose2)
        h_err = h_desired-self.z.get_position_meters()

        g = np.matrix(hl_new.A1.tolist()+[good_location[2,0]+h_err]).T
        g = mcf.torsoTglobal(mcf.globalTmecanum(g))
        angle_turned = pose2[2]-pose1[2]
        angle_error = tr.angle_within_mod180(turn_angle-angle_turned)
        self.segway_pose.set_origin() # re-origining for the manipulation behaviors
        return g, angle_error
 def IK_kdl(self,frame, q_init):
     nJnts = self.kdl_chains['nJnts']
     ik_solver = self.kdl_chains['ik_p']
     q = kdl.JntArray(nJnts)
     if ik_solver.CartToJnt(q_init,frame,q)>=0:
         for i in range(nJnts):
             q[i] = tr.angle_within_mod180(q[i])
         return q
     else:
         return None
 def IK_kdl(self,frame, q_init):
     nJnts = self.kdl_chains['nJnts']
     ik_solver = self.kdl_chains['ik_p']
     q = kdl.JntArray(nJnts)
     if ik_solver.CartToJnt(q_init,frame,q)>=0:
         for i in range(nJnts):
             q[i] = tr.angle_within_mod180(q[i])
         return q
     else:
         return None
Exemple #7
0
 def IK_kdl(self, arm, frame, q_init):
     """ IK, returns jointArray (None if impossible)
         frame - desired frame of the end effector
         q_init - initial guess for the joint angles. (JntArray)
     """
     nJnts = self.cody_kdl[arm]["nJnts"]
     ik_solver = self.cody_kdl[arm]["ik_p"]
     q = kdl.JntArray(nJnts)
     if ik_solver.CartToJnt(q_init, frame, q) >= 0:
         for i in range(nJnts):
             q[i] = tr.angle_within_mod180(q[i])
         return q
     else:
         if arm == "right_arm":
             ik_solver = self.cody_kdl[arm]["ik_p_nolim"]
             if ik_solver.CartToJnt(q_init, frame, q) >= 0:
                 for i in range(nJnts):
                     q[i] = tr.angle_within_mod180(q[i])
                 return q
         print "Error: could not calculate inverse kinematics"
         return None
Exemple #8
0
 def IK_kdl(self,arm,frame, q_init):
     ''' IK, returns jointArray (None if impossible)
         frame - desired frame of the end effector
         q_init - initial guess for the joint angles. (JntArray)
     '''
     nJnts = self.cody_kdl[arm]['nJnts']
     ik_solver = self.cody_kdl[arm]['ik_p']
     q = kdl.JntArray(nJnts)
     if ik_solver.CartToJnt(q_init,frame,q)>=0:
         for i in range(nJnts):
             q[i] = tr.angle_within_mod180(q[i])
         return q
     else:
         if arm == 'right_arm':
             ik_solver = self.cody_kdl[arm]['ik_p_nolim']
             if ik_solver.CartToJnt(q_init,frame,q)>=0:
                 for i in range(nJnts):
                     q[i] = tr.angle_within_mod180(q[i])
                 return q
         print 'Error: could not calculate inverse kinematics'
         return None
    goalB.motion_plan_request.group_name = 'right_arm'
    goalB.motion_plan_request.num_planning_attempts = 1
    goalB.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)

    goalB.motion_plan_request.planner_id = ''
    goalB.planner_service_name = 'ompl_planning/plan_kinematic_path'


    import roslib; roslib.load_manifest('darpa_m3')
    import sandbox_advait.pr2_arms as pa
    pr2_arms = pa.PR2Arms()
    raw_input('Move arm to goal location and hit ENTER')
    q = pr2_arms.get_joint_angles(0)
    raw_input('Move arm to start location and hit ENTER')

    q[6] = tr.angle_within_mod180(q[6])
    q[4] = tr.angle_within_mod180(q[4])
    for i in range(7):
        jc = JointConstraint()
        jc.joint_name = names[i]
        jc.position = q[i]
        jc.tolerance_below = 0.1
        jc.tolerance_above = 0.1
        goalB.motion_plan_request.goal_constraints.joint_constraints.append(jc)

    move_arm.send_goal(goalB)
    finished_within_time = move_arm.wait_for_result(rospy.Duration(200.0))

    if not finished_within_time:
        move_arm.cancel_goal()
        rospy.loginfo("Timed out achieving goal A")
Exemple #10
0
    goalB.motion_plan_request.group_name = 'right_arm'
    goalB.motion_plan_request.num_planning_attempts = 1
    goalB.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)

    goalB.motion_plan_request.planner_id = ''
    goalB.planner_service_name = 'ompl_planning/plan_kinematic_path'

    import roslib
    roslib.load_manifest('darpa_m3')
    import sandbox_advait.pr2_arms as pa
    pr2_arms = pa.PR2Arms()
    raw_input('Move arm to goal location and hit ENTER')
    q = pr2_arms.get_joint_angles(0)
    raw_input('Move arm to start location and hit ENTER')

    q[6] = tr.angle_within_mod180(q[6])
    q[4] = tr.angle_within_mod180(q[4])
    for i in range(7):
        jc = JointConstraint()
        jc.joint_name = names[i]
        jc.position = q[i]
        jc.tolerance_below = 0.1
        jc.tolerance_above = 0.1
        goalB.motion_plan_request.goal_constraints.joint_constraints.append(jc)

    move_arm.send_goal(goalB)
    finished_within_time = move_arm.wait_for_result(rospy.Duration(200.0))

    if not finished_within_time:
        move_arm.cancel_goal()
        rospy.loginfo("Timed out achieving goal A")
Exemple #11
0
                sturm_file.write(" ".join(map(str, p)))
                sturm_file.write('\n')

            sturm_file.write('\n')
            sturm_file.close()

        rad_guess = rad
        rad, cx, cy = fit_circle(rad_guess,
                                 x_guess,
                                 y_guess,
                                 pts_2d,
                                 method='fmin_bfgs',
                                 verbose=False)
        print 'rad, cx, cy:', rad, cx, cy
        c_ts = np.matrix([cx, cy, 0.]).T
        start_angle = tr.angle_within_mod180(
            math.atan2(pts_2d[1, 0] - cy, pts_2d[0, 0] - cx) - math.pi / 2)
        end_angle = tr.angle_within_mod180(
            math.atan2(pts_2d[1, -1] - cy, pts_2d[0, -1] - cx) - math.pi / 2)
        mpu.plot_circle(cx,
                        cy,
                        rad,
                        start_angle,
                        end_angle,
                        label='Actual\_opt',
                        color='r')

    if opt.icra_presentation_plot:
        mpu.set_figure_size(30, 20)
        rad = 1.0
        x_guess = pts_2d[0, 0]
        y_guess = pts_2d[1, 0] - rad
Exemple #12
0
            sturm_pts = cartesian_force_clean.p_list
            print 'len(sturm_pts):', len(sturm_pts)
            print 'len(pts_list):', len(pts_list)

            for i,p in enumerate(sturm_pts[1:]):
                sturm_file.write(" ".join(map(str,p)))
                sturm_file.write('\n')

            sturm_file.write('\n')
            sturm_file.close()

        rad_guess = rad
        rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
                                 method='fmin_bfgs',verbose=False)
        c_ts = np.matrix([cx, cy, 0.]).T
        start_angle = tr.angle_within_mod180(math.atan2(pts_2d[0,1]-cy,
                               pts_2d[0,0]-cx) - math.pi/2)
        end_angle = tr.angle_within_mod180(math.atan2(pts_2d[-1,1]-cy,
                               pts_2d[-1,0]-cx) - math.pi/2)
        mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
                        label='Actual\_opt', color='r')


    if opt.icra_presentation_plot:
        mpu.set_figure_size(30,20)
        rad = 1.0
        x_guess = pts_2d[0,0]
        y_guess = pts_2d[1,0] - rad

        rad_guess = rad
        rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
                                 method='fmin_bfgs',verbose=False)
Exemple #13
0
        rot_mat = tr.Rz(angle)[0:2,0:2]
        translation_mat = np.matrix([cx,cy]).T

        robot_width,robot_length = 0.1,0.2
        robot_x_list = [-robot_width/2,-robot_width/2,robot_width/2,robot_width/2,-robot_width/2]
        robot_y_list = [-robot_length/2,robot_length/2,robot_length/2,-robot_length/2,-robot_length/2]
        robot_mat = rot_mat*(np.matrix([robot_x_list,robot_y_list]) - translation_mat)
        mpu.plot_yx(robot_mat[1,:].A1,robot_mat[0,:].A1,linewidth=2,scatter_size=0,
                    color='k',label='torso')

        pts2d_actual = (np.matrix(actual_cartesian.p_list).T)[0:2]
        pts2d_actual_t = rot_mat *(pts2d_actual -  translation_mat)
        mpu.plot_yx(pts2d_actual_t[1,:].A1,pts2d_actual_t[0,:].A1,scatter_size=20,label='FK')

        end_pt = pts2d_actual_t[:,-1]
        end_angle = tr.angle_within_mod180(math.atan2(end_pt[1,0],end_pt[0,0])-math.radians(90))

        mpu.plot_circle(0,0,rad,0.,end_angle,label='Actual_opt',color='r')
        mpu.plot_radii(0,0,rad,0.,end_angle,interval=math.radians(15),color='r')
        pl.legend(loc='best')
        pl.axis('equal')

        if not(expt_plot):
            str_parts = fname.split('.')
            fig_name = str_parts[0]+'_robot_pose.png'
            pl.savefig(fig_name)
            pl.figure()
        else:
            pl.subplot(232)

        pl.text(0.1,0.15,d['info'])
Exemple #14
0
            print 'len(sturm_pts):', len(sturm_pts)
            print 'len(pts_list):', len(pts_list)

            for i,p in enumerate(sturm_pts[1:]):
                sturm_file.write(" ".join(map(str,p)))
                sturm_file.write('\n')

            sturm_file.write('\n')
            sturm_file.close()

        rad_guess = rad
        rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
                                 method='fmin_bfgs',verbose=False)
        print 'rad, cx, cy:', rad, cx, cy
        c_ts = np.matrix([cx, cy, 0.]).T
        start_angle = tr.angle_within_mod180(math.atan2(pts_2d[1,0]-cy,
                               pts_2d[0,0]-cx) - math.pi/2)
        end_angle = tr.angle_within_mod180(math.atan2(pts_2d[1,-1]-cy,
                               pts_2d[0,-1]-cx) - math.pi/2)
        mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
                        label='Actual\_opt', color='r')


    if opt.icra_presentation_plot:
        mpu.set_figure_size(30,20)
        rad = 1.0
        x_guess = pts_2d[0,0]
        y_guess = pts_2d[1,0] - rad

        rad_guess = rad
        rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
                                 method='fmin_bfgs',verbose=False)
            sturm_pts = cartesian_force_clean.p_list
            print 'len(sturm_pts):', len(sturm_pts)
            print 'len(pts_list):', len(pts_list)

            for i,p in enumerate(sturm_pts[1:]):
                sturm_file.write(" ".join(map(str,p)))
                sturm_file.write('\n')

            sturm_file.write('\n')
            sturm_file.close()

        rad_guess = rad
        rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
                                 method='fmin_bfgs',verbose=False)
        c_ts = np.matrix([cx, cy, 0.]).T
        start_angle = tr.angle_within_mod180(math.atan2(pts_2d[0,1]-cy,
                               pts_2d[0,0]-cx) - math.pi/2)
        end_angle = tr.angle_within_mod180(math.atan2(pts_2d[-1,1]-cy,
                               pts_2d[-1,0]-cx) - math.pi/2)
        mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
                        label='Actual\_opt', color='r')


    if opt.icra_presentation_plot:
        mpu.set_figure_size(30,20)
        rad = 1.0
        x_guess = pts_2d[0,0]
        y_guess = pts_2d[1,0] - rad

        rad_guess = rad
        rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
                                 method='fmin_bfgs',verbose=False)