diff --git a/python/examples/.clik.py.swp b/python/examples/.clik.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..170a055deeb26d0d8c333e4a1a4337f2328b14a5
Binary files /dev/null and b/python/examples/.clik.py.swp differ
diff --git a/python/examples/clik.py b/python/examples/clik.py
index 332ab5635b51dd8a0be82cc707ea1937f94dde90..5d728005134fec929b8099dadf6b6e21235d8804 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -31,7 +31,12 @@ if __name__ == "__main__":
else:
Mgoal = robot.defineGoalPointCLI()
#compliantMoveL(args, robot, Mgoal)
- moveL(args, robot, Mgoal)
+ if robot.robot_name != "yumi"
+ moveL(args, robot, Mgoal)
+ else:
+ goal_transform = pin.SE3.Identity()
+ goal_transform.translation[0] = 0.1
+ moveLDualArm(args, robot, Mgoal, goal_transform)
robot.closeGripper()
robot.openGripper()
if not args.pinocchio_only:
diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..af83ac7f5436cadd1f4a612479c3a2b4869838d2
Binary files /dev/null and b/python/ur_simple_control/clik/.clik.py.swp differ
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index a9eb3f1f591c6d984b1594473aff140ff612f947..5e5ec88f0d4c288ad7a7c164efadf74fe092bd31 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -187,6 +187,50 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
# hence the empty dict
return breakFlag, {}, log_item
+
+def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform, i, past_data):
+ """
+ controlLoopClikDualArm
+ ---------------
+ do point to point motion for each arm and its goal.
+ that goal is generated from a single goal that you pass,
+ and a transformation on the goal you also pass.
+ the transformation is done in goal's frame (goal is and has
+ to be an SE3 object).
+ the transform is designed for the left arm,
+ and its inverse is applied for the right arm.
+ """
+ breakFlag = False
+ log_item = {}
+ q = robot.getQ()
+ T_w_e_left, T_w_e_right = robot.getT_w_e()
+ #
+ Mgoal_left = robot.Mgoal.act(goal_transform)
+ Mgoal_right = robot.Mgoal.act(goal_transform.inverse())
+
+ SEerror_left = T_w_e_left.actInv(Mgoal_left)
+ SEerror_right = T_w_e_right.actInv(Mgoal_right)
+
+ err_vector_left = pin.log6(SEerror_left).vector
+ err_vector_right = pin.log6(SEerror_right).vector
+
+ if (np.linalg.norm(err_vector_left) < robot.args.goal_error) and (np.linalg.norm(err_vector_right) < robot.args.goal_error):
+ breakFlag = True
+ J_left = pin.computeFrameJacobian(robot.model, robot.data, q, robot.l_ee_frame_id)
+ J_right = pin.computeFrameJacobian(robot.model, robot.data, q, robot.r_ee_frame_id)
+ # compute the joint velocities based on controller you passed
+ qd_left = clik_controller(J_left, err_vector_left)
+ qd_right = clik_controller(J_right, err_vector_right)
+ qd = qd_left + qd_right
+ robot.sendQd(qd)
+
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ log_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
+ # we're not saving here, but need to respect the API,
+ # hence the empty dict
+ return breakFlag, {}, log_item
+
def controlLoopClikArmOnly(robot, clik_controller, i, past_data):
breakFlag = False
log_item = {}
@@ -291,6 +335,27 @@ def moveL(args, robot : RobotManager, goal_point):
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
loop_manager.run()
+def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform):
+ """
+ moveL
+ -----
+ does moveL.
+ send a SE3 object as goal point.
+ if you don't care about rotation, make it np.zeros((3,3))
+ """
+ #assert type(goal_point) == pin.pinocchio_pywrap.SE3
+ robot.Mgoal = copy.deepcopy(goal_point)
+ clik_controller = getClikController(args)
+ controlLoop = partial(controlLoopClikDualArm, robot, clik_controller, goal_transform)
+ # we're not using any past data or logging, hence the empty arguments
+ log_item = {
+ 'qs' : np.zeros(robot.model.nq),
+ 'dqs' : np.zeros(robot.model.nv),
+ 'dqs_cmd' : np.zeros(robot.model.nv),
+ }
+ loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
+ loop_manager.run()
+
# TODO: implement
def moveLFollowingLine(args, robot, goal_point):
"""
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index a9cd4475810cc6f52fc7cc8824e87f510b381b3d..1360ab9d4d3ffbcce64a81494774355bd3f909a1 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -563,20 +563,37 @@ class RobotManager:
return self.v_q.copy()
def getT_w_e(self, q_given=None):
- if q_given is None:
- return self.T_w_e.copy()
+ if self.robot_name != "yumi":
+ if q_given is None:
+ return self.T_w_e.copy()
+ else:
+ assert type(q_given) is np.ndarray
+ # calling these here is ok because we rely
+ # on robotmanager attributes instead of model.something
+ # (which is copying data, but fully separates state and computation,
+ # which is important in situations like this)
+ pin.forwardKinematics(self.model, self.data, q_given,
+ np.zeros(self.model.nv), np.zeros(self.model.nv))
+ # NOTE: this also returns the frame, so less copying possible
+ #pin.updateFramePlacements(self.model, self.data)
+ pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+ return self.data.oMf[self.ee_frame_id].copy()
else:
- assert type(q_given) is np.ndarray
- # calling these here is ok because we rely
- # on robotmanager attributes instead of model.something
- # (which is copying data, but fully separates state and computation,
- # which is important in situations like this)
- pin.forwardKinematics(self.model, self.data, q_given,
- np.zeros(self.model.nv), np.zeros(self.model.nv))
- # NOTE: this also returns the frame, so less copying possible
- #pin.updateFramePlacements(self.model, self.data)
- pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
- return self.data.oMf[self.ee_frame_id].copy()
+ if q_given is None:
+ return self.T_w_e_left.copy(), self.T_w_e_right.copy().copy()
+ else:
+ assert type(q_given) is np.ndarray
+ # calling these here is ok because we rely
+ # on robotmanager attributes instead of model.something
+ # (which is copying data, but fully separates state and computation,
+ # which is important in situations like this)
+ pin.forwardKinematics(self.model, self.data, q_given,
+ np.zeros(self.model.nv), np.zeros(self.model.nv))
+ # NOTE: this also returns the frame, so less copying possible
+ #pin.updateFramePlacements(self.model, self.data)
+ pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
+ pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
+ return self.data.oMf[self.l_ee_frame_id].copy(), self.data.oMf[self.r_ee_frame_id].copy()
# this is in EE frame by default (handled in step which
@@ -650,8 +667,14 @@ class RobotManager:
# (includes forward kinematics, all jacobians, all dynamics terms, energies)
#pin.computeAllTerms(self.model, self.data, self.q, self.v_q)
pin.forwardKinematics(self.model, self.data, self.q, self.v_q)
- pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
- self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
+ if self.robot_name != "yumi":
+ pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+ self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
+ else:
+ pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
+ pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
+ self.T_w_e_left = self.data.oMf[self.l_ee_frame_id].copy()
+ self.T_w_e_right = self.data.oMf[self.r_ee_frame_id].copy()
# wrench in EE should obviously be the default
self._getWrenchInEE(step_called=True)
# this isn't real because we're on a velocity-controlled robot,
@@ -750,8 +773,14 @@ class RobotManager:
q = np.array(q)
self.q = q
pin.forwardKinematics(self.model, self.data, q)
- pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
- self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
+ if self.robot_name != "yumi":
+ pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+ self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
+ else:
+ pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
+ pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
+ self.T_w_e_left = self.data.oMf[self.l_ee_frame_id].copy()
+ self.T_w_e_right = self.data.oMf[self.r_ee_frame_id].copy()
def _getQd(self):
"""