To-do List

[jiangjun0105]

• [x] ペッパーの手を引っ張って、トルクの値を見る
• [x] Rvizでペッパーのトルクの値を矢印で表示する
• [x] ペッパーに矢印の向きに移動してもらう
  • [x] ペッパーが矢印のむきに回るプログラムを作る
  • [x] ペッパーが回らない原因を見つける
  • [x] 安全性を保障するためプログラムをつくる
  • [x] ペッパーが矢印の向きに移動するプログラムを作る
• [x] 手のタッチセンサを使う
  • [x] NAOqi でタッチセンサを使う
  • [x] うまく行ければ、ROSのコードを書く
  • [x] euslispのコードを書く
• [x] stiffnessの問題
• [ ] 手先の座標変化によって移動する
  • [x] 前に伸ばすと、進む
  • [x] 横に伸ばすと、回る
  • [ ] 手先の高さによって速度が変わる
  • [x] 連続的に進めるように
  • [x] 時々自分で少し戻る問題を解決する
  • [ ] 音声は行動より遅い問題を解決する
• [ ]

cc: @kochigami

[kochigami]

About No.1 to do (check torque values by puling pepper's arm): from rostopic echo /joint_states, the order of joint name in the list is as follows

'HeadYaw', 'HeadPitch', 'LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'LWristYaw', 'LHand', 'HipRoll', 'HipPitch', 'KneePitch', 'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll', 'RWristYaw', 'RHand', 'WheelFL', 'WheelFR', 'WheelB'

1. In order to use (send ri :error-vector), we have to add some codes by following [naoqi_joint_states.py] create dummy effort (error) from sensord joint angle and desired joint angle #37 .
2. how to change stiffness of pepper (send ri :send-stiffness-controller 0.5) etc
3. what I did

setq value1 (send *ri* :error-vector)
;; move pepper's right arm
setq value2 (send *ri* :error-vector)
mapcar #'- (coerce value1 cons) (coerce value2 cons)

the results are as follows (number 11-16 out of 20 (0-19) relate to right arm)

stiffness 1.0:
(0.0 -0.087891 0.0 0.0 0.0 0.0 0.087891 0.0 0.0 0.087891 0.0 1.66993 0.351563 -0.439454 (-2.63671) -1.05469 -0.050345 0.0 0.0 0.0) :rarm :elbow-r
(0.0 -0.351563 0.0 0.0 0.0 0.0 0.0 0.0 -0.087891 -0.087891 0.0 1.93357 1.23047 0.087891 (-2.98828) 1.14258 0.0 0.0 0.0 0.0) :rarm :elbow-r

stiffness 0.8:
(0.0 -0.615236 0.0 0.0 0.087891 0.087891 0.087891 0.050345 -0.175782 0.263673 1.05469 (9.66797) 1.75782 0.087891 -3.51562 3.25199 0.0 0.0 0.0 0.0) :rarm :shoulder-p
(0.0 -0.439454 0.087891 0.0 0.087891 0.0 0.0 0.0 -0.175782 0.263673 1.23047 (13.0078) 1.75782 -0.9668 -3.60351 3.07621 0.0 0.0 0.0 0.0) :rarm :shoulder-p
When I took a short breath and set value2,
(0.0 -0.351563 0.0 0.0 -0.087891 0.087891 0.0 0.0 -0.439454 0.263673 1.14258 (12.9199) 1.84571 -1.05469 -3.86718 -3.95513 0.0 0.0 0.0 0.0) :rarm :shoulder-p

stiffness 0.5
(0.0 -0.087891 0.0 0.0 -0.087891 0.087891 -3.335053e-09 0.0 -0.527345 0.263673 
0.439454 (16.9629) 1.75782 -0.878909 -9.75586 0.0 0.0 0.0 0.0 0.0) :rarm :shoulder-r
(0.0 0.0 0.0 0.0 -0.087891 0.0 0.0 0.050345 -0.263673 0.0 0.351563 (12.6562) 2.81251 -0.263673 -4.13086 0.878922 0.0 0.0 0.0 0.0) :rarm :shoulder-p
(0.0 0.175782 0.0 0.0 0.0 -0.087891 0.0 0.0 -0.087891 0.439441 0.351563 (7.03124)
 1.66993 -1.05469 -3.16406 -3.86723 0.0 0.0 0.0 0.0) :rarm :shoulder-p
(0.0 -0.087891 0.0 0.0 0.0 0.0 0.0 0.0 -0.263673 0.175782 0.527345 (11.9531) 2.02149 -0.9668 -3.33984 0.087891 0.0 0.0 0.0 0.0) :rarm :shoulder-p
(0.0 -0.527345 0.351563 0.0 0.0 0.087891 0.0 0.0 -0.087891 0.527345 1.23047 (12.3926) 2.63673 -0.9668 -3.25195 -3.77933 0.0 0.0 0.0 0.0) :rarm :shoulder-p

stiffness 1.0 & move neither arm
(0.0 0.0 0.0 0.0 0.0 -0.087891 0.0 0.0 -0.087891 -0.087891 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0)
(0.0 0.0 0.0 0.0 0.0 0.087891 0.0 0.0 0.0 0.087891 0.0 0.0 0.0 -0.087891 0.0 0.0 -0.050345 0.0 0.0 0.0)

stiffness 0.8 & left arm
(0.0 -0.263673 (8.70118) 2.9004 -0.087891 2.7246 -0.791018 0.0 0.175782 -0.263673 0.351563 0.0 0.175782 -0.087891 0.0 0.0 0.0 0.0 0.0 0.0) :larm :shoulder-p
(0.0 -0.351563 (9.14063) -1.49413 0.087891 4.21875 0.263673 0.0 0.087891 -0.087891 1.05469 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0) :larm :shoulder-p

[kochigami]

We found that if we pulled pepper's arm in a horizontal direction, shoulder-r torque becomes largest, if we pulled pepper's arm in a vertical direction, shoulder-p torque becomes largest if we pulled pepper's arm in both horizontal and vertical direction, both torque values become largest.

[jiangjun0105]

[jiangjun0105]

[kochigami]

We find it is difficult to use virtual force publisher because we can't get correct stable data when using it to pepper even though we keep pulling pepper's arm. Instead, we gather a lot of data. We need theta (angle), shoulder-p & shoulder-r ratio (problem: how to get the theta?). Final goal is to enable pepper to follow us from all the direction, but at first, we will start pepper's rotation in a pulling direction.

What we discussed in 6/16: how to gather a lot of data set? -> we can get the error-vector of shoulder-p and shoulder-r , but how to get theta? -> theta : gained from the difference of model's and real robot's angle-vector.

how to save data? -> publish as topic and save them in a terminal

the question: could pepper detect the direction people want him or her to move if people pull pepper's arm in every direction? -> it is impossible or difficult

our conclusion: Using the error-vector as the determining factor of the direction people want pepper to move is difficult. However, we can use it as another factor. I think the size or ratio of the difference of shoulder-p or -r error value(before) and shoulder-p or -r error value(after) can be used as how far pepper moves.

[chiwunau]

pepper's arm is very lightweight and can be easily moved by human so why don't you use 1.the pointing direction (projected on the ground?) of the forearm
or 2.different vector of eus-model hand position and real-robot hand position. for example

(setq p1 (send *pepper* :larm :end-coords :copy-worldcoords))
;;displaced by human
(send *pepper* :angle-vector (send *ri* :state :potentio-vector))
(setq p2 (send *pepper* :larm :end-coords :copy-worldcoords))
(setq diff-v (v- (send p1 :world-pos) (send p2 :world-pos))  ;;not sure

[kochigami]

@chiwunau Thank you very much. I tried writing a test code, it seemed to be going to work well.

@jiangjun0105 However, actually I couldn't move pepper because I fall into another problem ( about :go-pos method in loop doesn' t work in my program)... Today, I'll try looking into jsk_demo program which seems helpful for me.

[chiwunau]

@jiangjun0105

will this work? https://gist.github.com/chiwunau/5abe7220c5d7ee963b81

[kochigami]

I tried @chiwunau 's suggestion. Thank you very much!! I think I didn't succeed it at first because :go-pos timing is too fast to follow. Now I set (ros::rate 5). Actually it seemed to work well, but unfortunately, pepper always changes his or her arm position when pepper rotates... (Always the difference of angle-vector is created and pepper rotates, and this never stops.) If I use virtual force publisher, the problem may be same. Pepper must move his or her arm when rotating. Therefore, I have to find the method to avoid using his or her arm when rotating.

[kochigami]

In order to stop pepper's arm movement: http://doc.aldebaran.com/2-1/naoqi/motion/control-walk-api.html#ALMotionProxy::setMoveArmsEnabled__bCR.bCR

We may have to split two modes (one is getting the difference of vector, the other is pepper's movement) because getting vector and moving pepper at the same time didn't succeed (:go-pos was never executed and this might relate to the continuous change of the difference of vector).
Now we look into /jsk-ros-pkg/jsk_demos/interactive_behavior_201409 package for modifying codes.

[jiangjun0105]

@kochigami I made some changes of your program, which is show as follow: And when I ran this program, I got the result as follow:

The message showed the :go-pos succeeded and pepper rotated every time I pull its arm, but the fact is some times pepper rotated but some times not. I don't know why.

[jiangjun0105]

By the way, another discovery is when the touch sensors, which are on pepper's feet are activated, pepper will not rotate at all. It maybe a self-protect mechanism. So I removed the chair away.

[kochigami]

@jiangjun0105 Thank you for your advice! I found the same problem about pepper's wheel. We maybe have to find the way of 1. checking pepper's wheel servo status, 2. changing pepper's wheel servo on.

When I sent :go-pos method for several times in my program, pepper rotated, but suddenly pepper didn't rotate because wheel servo turned off. Actually, I cannot know it is caused by my program or pepper's self-mechanism. In addition, I couldn't find method of checking the status of wheel servo. (I found the strange situation in which the value of wheel servo indicates 0 (off) every time, but sometimes pepper moves by :go-pos method...)

[kochigami]

I found pepper's self protection system for a collision. When I touched pepper's arm once, pepper rotated. However, at the same time pepper thought it was "a collision" and servo off pepper's left arm (which I touched on) and maybe also wheel. Therefore it seems that pepper stopped to rotate.

-> I also found that pepper doesn't move when someone touched pepper's arm. (wheel servo turns off)

How to deactivate arms anti collision: http://doc.aldebaran.com/2-1/naoqi/motion/reflexes-collision-avoidance-api.html?highlight=collision#ALMotionProxy::getCollisionProtectionEnabled How to deactivate movement & all anti collision: http://doc.aldebaran.com/2-1/naoqi/motion/reflexes-external-collision-api.html?highlight=collision#ALMotionProxy::getExternalCollisionProtectionEnabled (but this requires the owner consent... what's this??)

[kochigami]

To do first: Finding the cause of servo off of pepper's wheel forget ROS & use NAOqi directly if possible: move pepper to a place which is no obstacle

In the situation that pepper moving by using MoveTo method:　 Try: if we approach pepper, he or she stops? (due to laser sensor or something) Try: if we touch pepper with avoiding detected by sensor, he or she stops? (due to touch sensor) Try: If we make the distance of sensor 0 cm, what does happen?

[kochigami]

I experimented two question above(if we approach pepper, he or she stops? and if we touch pepper with avoiding detected by sensor, he or she stops?).

pepper's pose: reset-pose
number of experiment in each condition -> 5 times

succeed-> pepper continues to rotate
fail -> pepper (and program) stops 

1.approach toward pepper (in front of pepper)
1-1.30 cm -> 5/5 succeeded
1-2.10 cm -> 5/5 failed

2. touch (I grasped strongly rather than touched pepper's both arms from more than 30 cm distance)
2-1. hand 5/5 failed
2-2. wrist 
2-2-1. hand side 5/5 failed
2-2-2. elbow-side 5/5 succeeded
2-3. joint between wrist and elbow 5/5 succeeded

3. torque (I pulled pepper's joint between wrist and elbow)
3-1. right arm 5/5 failed
3-2. left arm 5/5 succeeded 
I don't know why there is a difference.
3-3. another pose (like picture) (I pulled pepper's left joint between wrist and elbow)
5/5 failed
NAOqi Moveto method also failed in this pose. 
3-4. reset-pose, pulled pepper's left joint between wrist and elbow
5/5 succeeded :), but this situation is somehow strange.

I think there are three factors which make pepper's wheel servo off, and they are 1. approaching distance, 2. touch, 3. torque. The reason why we failed might be mainly 2 and 3.

[kochigami]

I found "consent for the deactivation of safety reflexes (the owner consent)" in http://doc.aldebaran.com/2-1/nao/nao-webpage.html, but it may relate to only falling of the robot and doesn't relate to wheel servo on/off. I haven't found the program which deactivate pepper's wheel servo yet.

By the way, change the stiffness from 1.0 to 0.5 is better for pulling pepper's arm. (0.1 is too weak to sustain the status of servo on) In addition, I want to create the function of informing pepper's wheel servo status and enabling us to servo on it easily in the future.

[jiangjun0105]

I read the beginner guide of pepper, which is https://community.aldebaran.com/en/resources/documents and found some information in it, but nothing very helpful.

First, The Charging Flap is located at the base and has two functions:

• It allows you to charge Pepper’s battery (see Chapter 7.6 Charging).
• It also deactivates the wheels as a security measure. When open, the Charging Flap prevents Pepper from activating his wheels.

I remember we took off the charger, but did we close the charging flap?

Second, the area that pepper's sensors can cover is it maybe helps when we do experiment.

[jiangjun0105]

It will be great if you can make the function informing the wheel servo status! But I think we still have to find the reason why sometimes it turns off by itself, especially during the experiment.

[kochigami]

Thank you for your advice!! :)

I remember we took off the charger, but did we close the charging flap? -> Yes, we closed the charging flap, but pepper stops moving suddenly by unknown reason. And I think there are two or three factors (laser sensor, touch sensor, (or huge torque)).

What I feel strange is that there is no explanation that the servo status of pepper's wheel becomes "off" by some factor, even though there are some explanations that the speed of movement of arms becomes slow when some collisions are detected or so.

Anyway, let's discuss and work on it together later !!

[kochigami]

I found the way of keeping pepper's wheel servo on.

1. "consent for the deactivation of safety reflexes (the owner consent)" in http://doc.aldebaran.com/2-1/nao/nao-webpage.html
2. Then, "collision protection disabled on "Move" in http://doc.aldebaran.com/2-1/naoqi/motion/reflexes-external-collision-api.html?highlight=collision%20disabled#ALMotionProxy::setExternalCollisionProtectionEnabled__ssCR.bCR Now I can move pepper with grasping his or her hand :)

However, in terms of safety, this method is sometimes dangerous. For example, Pepper can move with his or her charger flap open, pepper continues to move if the bumper collides obstacles, pepper can't take a safety pose if someone bend pepper's body.

Although there are some problems, we can go to next step! (I want to move with pepper because what I did is only rotating pepper in a direction I pulled pepper's arm.)

[jiangjun0105]

Great! You said you can move pepper with grasping its hand; which version of program did you use when you did the test? I just want to know if that pepper didn't work very well was all because of the safety reflexes, and whether in fact our program has no problem at all. And about the safety, I also think is a bit of dangerous if we turn off all the safety reflexes functions. Maybe later we can try to off the functions partly, not all of them. But I prefer to do the next step of our task first, I really can't wait to pull pepper to walk around! I think the next step can go faster and more smoothly!

[kochigami]

which version of program did you use when you did the test? -> I tested my new code which includes two modes of pepper's behavior, but I think your program 3 days ago will also work. Let's try it!! :)

Maybe later we can try to off the functions partly, not all of them. -> If there is another option, let's use them partly (We need to search for it more). The function I conducted now is only one function, and therefore we can't separate it, but I think by writing new simple function, we can use bumper sensor to avoid collision.

Maybe we have to write such a code for avoiding collision before we go to next step, because I tested yesterday, pepper continued to move even though his or her wheel collided with some obstacles, which is too dangerous for pepper and us.

[jiangjun0105]

Maybe we have to write such a code for avoiding collision before we go to next step, because I tested >yesterday, pepper continued to move even though his or her wheel collided with some obstacles, >which is too dangerous for pepper and us.

Alright, your are right, and we should put safety first :)

[jiangjun0105]

I updated the to-do-list, adding all things we did so that we can record them and have more sense of achievement : )

[kochigami]

Safety: I found that Pepper can't stop once he or she starts to move by using :go-pos method. Pepper can stop only before he or she does not move yet. It is still dangerous. -> it may relate to Pepper's :go-pos method. I will write another issue.

Movement: I found that Pepper goes straight and backward repeatedly... I consider whether it will be solved by using more types of sensors. (ex: using virtual_force_publisher (obtaining virtual force or torque))

Stiffness: Still I can't change Pepper's stiffness on arm. I wonder why? Is it a software or hardware problem?

Sensors: I added some codes so we can gain pepper's back wheel sensor data, hand touch sensor data. I put the program on our directory. Unfortunately, we may be unable to gain arm touch sensor value because Pepper does not seem to have touch sensors on his or her arm.

[kochigami]

@jiangjun0105 I found solutions to safety, movement (thank you very much for your advice!!) and stiffness problems. Now Pepper can walk with us (However, you may have a lot of unsatisfactory if you try it :)) I will tell you how my new program works later, and let's improve it together!!

[k-okada]

On Sat, Jul 4, 2015 at 12:17 AM, Kanae Kochigami notifications@github.com wrote:

Stiffness: Still I can't change Pepper's stiffness on arm. I wonder why? Is it a software or hardware problem?

please check "arm motions" section of

http://doc.aldebaran.com/2-1/naoqi/motion/control-walk.html#move-config

◉ Kei Okada

[kochigami]

Thank you very much for your advice. The reason that I couldn't change the stiffness of Pepper's arm by using :send-stiffness-controller method in pepper-interface.l is this method is only for nao.

Now I sent a pull request for it, and I checked it works well even though I deactivated Pepper's arm using the function written in http://doc.aldebaran.com/2-1/naoqi/motion/control-walk.html#move-config.

[kochigami]

memo

1. ROS node for movement detect API -> old message type (not compatible for 2.1 ver) I should repair this, but there is no time, so I ignore this now.
2. my new program detect hand sensor & movement detect at the same time -> not good detect order is important (ex first hand sensor, then movement detection) movement detection parameter should be changed pepper should go backward rotation method should be modified

[kochigami]

memo (bibo-roku) I modified nao_tactile.py and msg file for pepper, and added some node in nao_apps. In the future I would like to send pull-requests about them.

[kochigami]

@jiangjun0105 Please try catkin make your naoqi_msgs, not nao_apps!!

[jiangjun0105]

Today, when I ran the kochigami20140714.l , a error came out:

;; extending gcstack 0x51e7810[32738] --> 0x66ae9f0[65476] top=7faa
/opt/ros/indigo/share/euslisp/jskeus/eus/Linux64/bin/irteusgl: ERROR th=0 mismatch argument  in (send *ri* :send-stiffness-controller :larm 0.0)E: 

Do you know what it is and how to solve it?

Although I couldn't do experiment, I still got some progress :) I cleaned some old code and make the structure and indent more clear. Also change velocity depend how much the arm is pull forward. New version is kochigami20150715.l and I've committed it. I think you can see it in the /jiang If you can't, please let me know. But unfortunately because of the error, I haven't tested it yet. Hope I can solve all the environment problem and test the new program on Saturday :)

[k-okada]

please check if you have latest peppereus code, it should contain https://github.com/jsk-ros-pkg/jsk_robot/pull/367/files

roscd peppereus git show git branch git remote -v

and if your repository is old,run git pull to get latest code

◉ Kei Okada

On Wed, Jul 15, 2015 at 8:59 PM, Jiang Jun notifications@github.com wrote:

Today, when I ran the kochigami20140714.l , a error came out:

;; extending gcstack 0x51e7810[32738] --> 0x66ae9f0[65476] top=7faa /opt/ros/indigo/share/euslisp/jskeus/eus/Linux64/bin/irteusgl: ERROR th=0 mismatch argument in (send ri :send-stiffness-controller :larm 0.0)E:

Do you know what it is and how to solve it?

Although I couldn't do experiment, I still got some progress :) I cleaned some old code and make the structure and indent more clear. Also change velocity depend how much the arm is pull forward. New version is kochigami20150715.l and I've committed it. I think you can see it in the /jiang If you can't, please let me know. But unfortunately because of the error, I haven't tested it yet. Hope I can solve all the environment problem and test the new program on Saturday :)

— Reply to this email directly or view it on GitHub https://github.com/jsk-ros-pkg/jsk_robot/issues/348#issuecomment-121592473 .

[kochigami]

@jiangjun0105 Thank you very much for your contribution!! Sorry for the difficulty in building environment ( it is exactly my fault...)

I have read all of your comments on your code. I'll answer your questions later.

After going back to lab, I'll try adding speeding up functions more. (for example: 0.4 -> (0.6) -> 1.0)

[kochigami]

I found the speed of 1.0 is too dangerous. My shoes and umbrella got run over by Pepper :< I modified it to 0.7, but we still have to consider safety.

I have to tackle on the problem that suddenly pepper goes backward first tomorrow. (I think the cause is determining the direction based on the x value) Then, I'll rewrite the speeding up functions.

[kochigami]

I haven't solved yet how to solve the back forwarding problem...

However, I tried this program in a real demonstration environment. I arranged chairs to make a zigzag course to walk. When walking alone, it took 30 seconds to go and return. I recorded three rosbag files and had Pepper taken about 50 pictures while walking (not automatically, but reacted to my input). One session took 6-7 minutes. This is one of the cute pictures pepper took.

[kochigami]

solve the go backward problem speaking latency

simple course

[kochigami]

I found why pepper goes backward -> I'll test it later :) speaking latency-> change wait time (I don't know if it works well add code for right hand prepare camera attachment, document, slide, ask for air conditioner, send an e-mail make pepper's head down

tomorrow: rehearsal time schedule test 1set (record a rosbag -> move it into my usb, measure time, count steps) practice of interpretation

[kochigami]

I haven't understood why pepper goes backward (I am very sorry about it) After sending some e-mails, I'll re-consider it.

[kochigami]

We found :go-pos (moveTo) -> Pepper tries to keep the ordered distance (for example: in the condition that pepper goes forward first, then stops by using (send *ri* :go-pos 0 0 0), pepper moves extra distance before he or she totally stops. Therefore pepper goes backward to keep the ordered distance. )

:go-velocity (moveToward)-> it is a function which determines just pepper's speed, so it doesn't relate to distance. Pepper can stop suddenly without going backward / forward.

[chiwunau]

I just saw @kochigami SII paper. http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=7405071 About the pull hand motion, you said, "Comparing the current position of an end effector and that in a default pose"

For your information, in some paper I have read recently, it seem that "end effector in a default pose" have a very cool name " Cartesian equilibrium point" (Where an mechanism lies without existence of external forces) http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5509445&newsearch=true&queryText=Advait%20Jain

[kochigami]

Thank you very much for your information ! Cartesian equilibrium point ：直交座標系平衡点

[kochigami]

Hi, I think it's ok to close (we have done all to-dos!) , but I don't have the permission to do so. Would anyone close this issue when you have time? Sorry for my trouble.